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Abstract 

Design  and  Application  of  Robust  Reduced-Order  Hybrid  Position  and  Force  Con¬ 
trollers  for  a  Two-Link  Flexible  Manipulator 

by  David  E.  Bossert 

Chairperson  of  the  Supervisory  Committee:  Professor  Juris  Vagners 

Department  of  Aeronautics  and 
Astronautics 

This  research  focuses  on  the  design  and  application  of  robust  reduced-order  hy¬ 
brid  position  and  force  controllers  on  a  two-link  flexible  manipulator.  The  first  part 
of  the  research  examines  the  design  aspect  of  the  hybrid  controllers.  Traditionally, 
full-order  controllers  are  used  in  common  techniques  such  as  Linear-Quadratic- 
Gaussian  (LQG)  control.  If  the  plant  is  of  large  order,  then  the  resulting  controller  is 
also  of  large  order.  Full-order  controllers  have  the  disadvantages  of  requiring  large 
amounts  of  memory  as  well  as  using  excessive  computational  time.  In  addition, 
LQG-class  controllers  are  only  guaranteed  to  work  at  the  design  condition.  With  the 
use  of  the  direct-optimization  tool  SANDY,  reduced-order  controllers  are  designed 
directly  for  several  plant  conditions  (robustness).  The  experimental  results  show  that 
the  robust  reduced-order  SANDY  controllers  provide  comparable  performance  to  the 
full-order  LQG  controllers  in  the  applications  of  position  control,  force  control,  and 


hybrid  position  and  force  control. 

The  second  thrust  of  the  research  is  on  the  application  of  these  controllers.  In  a 
typical  manufacturing  task,  there  are  several  stages  involved.  First,  the  tool  must 
move  from  some  initial  starting  position  and  make  contact  with  a  surface,  and  then  it 
must  perform  some  contact  task  (constrained  motion),  and  finally  return  to  its  initial 
location  to  restart  the  procedure  on  the  next  item.  This  work  develops  a  framework 
which  allows  a  tool  to  move  to  an  unknown  surface  whose  exact  location  is  not 
known,  perform  a  hybrid  position  and  force  control  task  on  the  surface,  and  then  re¬ 
tract  to  the  starting  position.  The  complete  (unified)  hybrid  task  method  is  presented 
as  a  6-phase  process,  and  is  experimentally  evaluated  on  a  two-link  manipulator.  The 
technique  performs  well  experimentally  for  both  full-order  and  reduced-order  con¬ 
trollers  on  an  unknown  straight  line  surface  and  an  unknown  arc  surface.  Also,  the 
structure  could  easily  be  adapted  to  the  case  where  a  surface  shape  is  known  exactly. 
The  approach  shows  promise  in  many  manufacturing  applications. 
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1  Introduction 


1.1  Motivation 

An  area  of  importance  in  manufacturing  applications  is  hybrid  position  and  force 
control.  For  tasks  such  as  buffing,  polishing,  grinding,  painting,  etc.  the  force  applied 
by  the  tool  to  the  workpiece  needs  to  be  controlled  while  simultaneously  controlling 
the  position.  This  hybrid  control  is  also  known  as  constrained  motion  since  the  ma¬ 
nipulator  is  constrained  by  a  surface. 

In  addition,  the  tool  in  a  manufacturing  task  must  start  at  an  arbitrary  location  in 
free-space,  move  to  the  workpiece,  perform  the  task,  and  then  return  to  the  starting 
location.  This  complete  or  unified  task  must  thus  include  free-space  motion  to  get  to 
the  workpiece,  impact  control  to  successfully  make  contact  with  the  object,  con¬ 
strained  hybrid  position  and  force  control  to  perform  the  task,  and  some  way  to 
transition  back  to  free  space  motion. 

Many  of  the  current  applications  for  manipulators  consider  only  the  rigid  body 
dynamics  of  the  robot.  However,  all  rigid  bodies  have  some  vibrational  or  flexible 
modes  associated  with  them.  This  is  especially  true  for  lighter  weight  manipulators 
which  have  lower  frequency  modes  of  vibration,  especially  in  a  high-speed  applica¬ 
tion.  This  leads  to  an  issue  of  flexibility,  and  controlling  the  flexible  modes  of  light¬ 
weight  manipulators  is  an  area  of  keen  interest.  Thus,  a  good  design  must  address 
flexibilities  in  the  manipulator.  In  the  future,  applications  will  require  more  of  the  ro¬ 
botic  manipulators  as  performance  requirements  increase. 

Another  common  practice  in  control  of  industrial  manipulators  is  the  use  of  full- 
order  controllers.  Full-order  controllers  have  the  disadvantage  of  being  the  same  or¬ 
der  as  the  model  and  can  thus  take  up  large  amounts  of  memory  and  involve 
excessive  computational  time.  Reduced-order  controllers  designed  through  direct- 
optimization  allow  use  of  the  full-order  model  without  the  penalty  of  increased  con¬ 
troller  size. 

The  approach  taken  here  is  to  use  the  SANDY  direct-optimization  design  tool  to 
simultaneously  design  robust,  reduced-order  controllers.  Also,  this  optimization  al¬ 
lows  the  need  for  position  and  force  selection  matrices  to  be  considered  in  the 
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optimization  of  the  controllers  through  bandwidth  separation 

Finally,  most  manufacturing  tasks  are  completed  with  the  complete  knowledge  of 
the  workpiece  shape.  Sometimes,  it  takes  a  great  deal  of  time  for  the  exact  shape  to 
be  programmed.  In  addition,  each  piece  usually  varies  somewhat  and  requires  re¬ 
working  of  the  feedforward  position  based  on  the  shape.  An  alternative  to  this  is  to 
use  an  algorithm  which  follows  an  arbitrary  shape  with  no  a  priori  knowledge.  Thus, 
the  same  controllers  and  feedforward  references  could  be  used  for  many  surfaces. 

This  research  seeks  to  consider  all  the  issues  discussed  above.  Thus,  a  complete 
(unified)  hybrid  control  task  using  robust  reduced-order  controllers  to  control  a  flex¬ 
ible  two-link  manipulator  while  working  on  an  unknown  surface  at  an  unknown 
location  is  analyzed,  developed,  and  implemented.  The  test  bed  for  the  research  is  the 
University  of  Washington  Control  Systems  Laboratory’s  flexible  two-link  manipula¬ 
tor,  or  two-link  arm  (TLA)  for  short. 

1.2  Contributions 

The  contributions  of  this  dissertation  include  the  following: 

1.  Interfacing  the  TLA  manipulator  to  the  rapid-prototyping  AC100-C30  real¬ 
time  data  acquisition  and  control  system. 

2.  Performing  parameter  identification  of  the  TLA  manipulator,  and  performing 
experimental  validation  of  the  resulting  model  for  both  position  and  force  control. 

3 .  Developing  a  baseline  of  performance  for  the  TLA  manipulator  using  Propor¬ 
tional-Integral-Derivative  (PID)  position  and  force  control. 

4.  Designing,  simulating,  and  experimentally  comparing  full-order  Linear-Qua¬ 
dratic  Gaussian  (LQG),  LQG/Loop  Transfer  Recovery  (LTR),  and  Special 
Coordinate  Basis  LTR  position  controllers. 

5.  Designing,  simulating,  and  experimentally  comparing  reduced-order  LQG, 
LQG/LTR  and  SCB/LTR  controllers  to  full-order  position  controllers. 

6.  Designing,  simulating,  and  experimentally  comparing  full-order  and  re¬ 
duced-order  LQG  force  controllers. 

7.  Designing,  simulating,  and  experimentally  evaluating  direct  non-linear  opti- 
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mization  SANDY  position  and  force  controllers. 

8.  Designing,  simulating,  and  experimentally  validating  robust  reduced-order 
position  and  force  controllers  for  the  TLA. 

9.  Developing  an  impact  control  algorithm  to  allow  successful  touchdown  of  the 
TLA  onto  a  surface  whose  exact  location  is  not  known. 

10.  Developing  a  complete  (unified)  control  structure  to  allow  free-space  motion, 
surface  touchdown,  constrained  motion,  transition  to  free-space  motion,  and  return 
to  the  initial  starting  location. 

11.  Comparing  hybrid  direct-optimization  designs  by  superposition  and  simulta¬ 
neous  optimization. 

12.  Developing  a  surface  following  algorithm  to  allow  the  TLA  to  follow  a  sur¬ 
face  without  a  priori  knowledge  of  the  surface  while  maintaining  a  constant  force. 

1.3  Organization 

This  dissertation  is  divided  into  16  chapters  and  5  appendices.  Before  starting 
into  the  new  work  associated  with  this  dissertation,  a  literature  review  of  current  flex¬ 
ible  manipulator  force  and  position  control  is  presented  in  Chapter  2.  After 
establishing  the  current  research,  the  experimental  setup  for  this  research  is  described 
in  Chapter  3.  Next,  the  dynamics  model  for  position  control  is  discussed  in  Chapter 
4,  leading  to  the  PID  control  design  and  experiments  presented  in  Chapter  5.  Con¬ 
tinuing,  the  position  design  is  extended  to  LQG-class  controller  designs  and 
experiments  in  Chapter  6.  Another  position  control  design  technique,  SANDY  direct- 
optimization  design,  is  described  in  Chapter  7  and  the  subsequent  position  control  ap¬ 
plication  is  presented  in  Chapter  8. 

The  position  control  design  is  extended  to  an  end-point  framework  with  SANDY 
in  Chapter  9  before  the  dissertation  covers  a  hybrid  control  overview  in  Chapter  10. 
Next,  Chapter  1 1  explains  the  force  control  model  and  design  while  Chapter  12  dis¬ 
cusses  the  actual  hybrid  control  designs.  Logically  following  the  hybrid  control 
design  is  the  surface  following  experiment  coverage  in  Chapter  13.  After  successful¬ 
ly  following  a  surface  while  starting  in  contact,  the  process  of  surface  touchdown  is 
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explained  in  Chapter  14.  Finally,  the  complete  hybrid  task  is  described  in  Chapter  15 
while  conclusions  are  presented  in  Chapter  16. 

The  appendices  contain  additional  information  on  several  topics  that  might  be  of 
interest  to  some  readers.  Appendix  A  contains  the  arm  modelling  details  not  included 
in  Chapter  4  while  Appendix  B  presents  additional  position  experiment  plots  for  the 
SANDY  and  PID  controllers.  Similarly,  additional  LQG-class  position  experiment 
plots  are  found  in  Appendix  C  while  Appendix  D  displays  the  additional  end-point 
tracking  plots  from  Chapter  9.  Finally,  Appendix  E  presents  some  of  the  MATLAB 
m-files  developed  for  this  work. 


2  Literature  Review 


In  order  to  give  credit  to  appropriate  people,  and  to  avoid  duplication  of  previous 
work,  an  extensive  literature  search  is  done.  In  particular,  a  review  of  all  work  on 
multiple-link  flexible  robotic  manipulator  theory,  simulation,  and  experimentation  is 
accomplished.  In  addition,  force  control,  hybrid  force  and  position  control,  and  sur¬ 
face  following  applications  are  examined. 

Flexible  robotic  manipulators  have  been  studied  a  great  deal  recently  due  to  more 
stringent  requirements.  Space  stmcture  applications  usually  require  low-weight  due 
to  the  high  cost  of  sending  heavy  payloads  into  space.  Space  manipulators  must  be 
light  for  the  same  reason,  and  therefore  flexibility  issues  become  important.  One  ex¬ 
ample  of  an  existing  flexible  space  manipulator  is  the  Shuttle  Remote  Manipulator 
System  built  by  Spar  Aerospace  Limited  for  the  Space  Shuttle.  Some  of  the  new  re¬ 
quirements  driving  flexible  manipulator  technology  are  lower  arm  cost,  higher 
motion  speeds,  and  improved  mobility  [4]. 

2.1  Current  Flexible  Manipulator  Facilities 

Rigid  manipulator  control  has  been  well-studied  by  many  people  [6]  [40]  [55]. 
The  key  difference  between  the  traditional  rigid  manipulators  and  the  lightweight 
flexible  manipulators  lies  in  the  elastic  modes  associated  with  the  flexible  manipula¬ 
tors.  For  this  reason,  many  flexible  manipulator  facilities  have  been  established. 
Much  of  the  current  research  has  been  done  at  Stanford  University  [56]  [57],  and  the 
University  of  Washington’s  Two-Link  Arm  (TLA)  is  based  on  Stanford’s  design. 
Among  the  other  locations  with  experimental  planar  two-link  flexible  manipulators 
are  Martin  Marietta  [65],  Sandia  National  Laboratories  [21],  the  USAF  Phillips  Lab¬ 
oratory  [19],  and  the  Polytechnic  Institute  [33]. 

2.2  Current  Flexible  Manipulator  Applications 

There  have  been  many  contributors  to  flexible  manipulator  research.  Some  ap¬ 
proaches  have  focused  on  adaptive  control  [4]  [24]  [13].  Others  have  focused  on 
linear  programming  [66],  self-tuning  controllers  [32],  sliding-mode  control  [14], 
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nonlinear  model-based  control  [58],  and  gain-scheduled  controllers  [51].  In  ad¬ 
dition,  use  of  rigid-body  based  controllers  with  input  preshaping  for  flexible 
manipulators  has  been  investigated  [33],  as  well  as  control  of  flexible  arms  with  fric¬ 
tion  in  the  joints  [23].  Also,  some  studies  have  focused  on  inversion-based  controllers 
[43].  Stanford  University  has  also  been  very  active  in  the  area  of  flexible  manipulator 
end-point  control  [57]. 

2.3  Constrained  Motion  Control 

A  robot  working  in  contact  with  a  surface  is  called  a  constrained  motion  problem 
if  the  surface  is  rigid,  or  a  “compliant  motion  task”  for  a  compliant  surface  [53]. 
There  has  been  a  great  deal  of  work  on  the  constrained  motion  problem.  In  fact,  a  lit¬ 
erature  search  on  “force  control”  returned  over  1500  documents!  The  two  major  areas 
are  impedance  control  [16] [37]  and  hybrid  position  and  force  control  [61].  The  first 
force  control  stability  analysis,  found  in  [67],  showed  that  a  trade-off  exists  between 
the  force  gains  and  the  environment  stiffness.  Others  attribute  the  dynamic  instability 
of  force  control  to  higher-order  dynamics  and  conclude  that  there  must  be  some  com¬ 
pliancy  in  either  the  robot  or  the  environment  [1].  A  stable  hybrid  approach  based  on 
the  original  hybrid  scheme  was  developed  by  Fisher  [25].  This  “correct  formulation” 
uses  pseudo-inverse  matrices  to  calculate  the  angle  error  vectors. 

Another  approach  developed  by  Matsuno  [49]  applied  perturbation  techniques  to 
a  multi-link  flexible  manipulator.  Also,  Cai  [12]  used  a  Lyapunov  approach  on  a  rigid 
manipulator  to  design  his  hybrid  control  scheme.  In  addition,  Kwan  used  neural  nets 
to  tackle  the  constrained  motion  problem  [36]. 

Stanford  has  been  very  involved  in  force  control  research  for  flexible  manipula¬ 
tors.  Maples  [48]  conducted  force  control  research  on  a  single-link  flexible 
manipulator.  This  was  extended  by  Kraft  [34],  who  added  a  mini-manipulator  on  the 
end  of  a  single-link  manipulator  and  experimentally  implemented  force  and  position 
control  algorithms.  The  most  recent  work  by  Ballhaus  [3]  extended  the  force  and  po¬ 
sition  control  to  a  two-link  flexible  manipulator  with  a  mini-manipulator  on  the  end. 
All  of  this  research  was  done  using  full-order  LQG  regulators,  and  no  effort  was 
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made  to  minimize  the  order  of  the  controllers.  Another  method  uses  a  transformation 
to  simultaneously  control  both  position  and  force  [29], 

Also,  there  has  been  some  work  with  the  surface  touchdown  problem,  sometimes 
called  impact  control.  One  approach  decreased  the  initial  force  command  by  input 
command  preshaping  [38].  Another  implemented  a  discontinuous  controller  to  re¬ 
duce  the  problems  of  impact  stability  [53].  Finally,  another  paper  considered  a 
compliant  wrist  and  tunable  PID  controller  [47]. 

Kraft  s  approach  [34]  involved  independently  designing  position  and  force  con¬ 
trollers,  and  then  just  adding  the  torque  commands  together  to  produce  one  hybrid 
signal.  Kraft’s  approach  is  similar  to  basic  hybrid  control  described  in  [61],  but  uses 
a  low-friction  roller  to  measure  the  surface  normal  (and  surface  tangential  direction) 
directly  instead  of  using  selection  matrices.  This  is  done  because  it  is  easier  to  track 
surfaces  without  prior  knowledge  of  the  surface  shape,  which  was  the  object  of  his 
research. 

2.4  Surface  Following  Applications 

There  are  many  applications  where  a  robotic  manipulator  is  required  to  follow  a 
surface  while  applying  a  constant  force  to  perform  a  task.  These  applications  include 
buffing,  grinding,  polishing,  or  painting.  The  normal  approach  to  tackle  these  prob¬ 
lems  is  to  leam  the  desired  trajectory  and  then  perform  the  task.  The  obvious  problem 
here  is  that  every  new  shape  requires  new  trajectory  learning. 

Some  of  the  current  methods  for  learning  the  trajectory  are  learning  control  [42] 
and  genetic  techniques  [27].  Another  approach  for  surface  uncertainty  is  fuzzy  com¬ 
pliance  control  [60].  Others  have  focused  on  trying  to  follow  a  surface  through 
differential  workpiece  models  [18].  Finally,  some  have  tried  to  follow  an  unknown 
surface  using  range  sensors  [59]  [54]. 

A  promising  approach  involving  hybrid  force  and  position  control  and  was  devel¬ 
oped  by  Kraft  [34]  for  a  single-link  flexible  manipulator  with  a  mini-manipulator 
mounted  at  the  end.  This  approaeh  uses  a  low-friction  roller  which  senses  essentially 
the  surface  normal  force  and  then  commands  a  position  reference  along  the  surface 
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tangent. 

2.5  The  Complete  (Unified)  Hybrid  Task 

The  complete  or  unified  hybrid  task  involves  starting  at  a  point  in  free-space,  ac¬ 
quiring  the  workpiece  during  the  impact  stage,  performing  the  hybrid  or  constrained 
task,  and  then  returning  to  free-space  motion.  Several  researchers  have  considered 
this  area.  Mandal  and  Payandah  [47]  conduct  an  extensive  review  of  various  compo¬ 
nents  of  the  unified  task,  and  tie  the  stages  together  with  a  knowledge-based  tuning 
of  gains.  Also,  Khatib  and  Burdick  [31]  propose  a  unified  approach  based  on  an  op¬ 
erational  space  formulation.  These  and  others  are  either  complex  or  require  different 
controllers  for  each  stage. 

2.6  Summary 

The  preceding  sections  show  that  flexible  manipulator  dynamics  and  position 
control  have  been  widely  studied.  Also,  many  position  control  techniques  have  been 
applied  to  flexible  multi-link  and  rigid  manipulators.  None  of  the  work  surveyed  con¬ 
sidered  robust  reduced-order  position  control  of  flexible  two-link  manipulators. 

Force  control,  also,  has  been  widely  studied.  Force  control,  when  used  in  con¬ 
junction  with  position  control  at  a  surface  is  known  as  the  constrained  motion 
problem.  Many  techniques  have  been  studied,  but  once  again  none  have  considered 
robust  reduced-order  designs. 

Finally,  the  complete  or  unified  hybrid  tasks  surveyed  required  modification  of 
the  controllers  for  each  phase  of  the  task  or  were  very  difficult  to  implement.  Thus, 
a  simple  unified  approach  which  uses  the  same  controllers  for  all  the  phases  would 
be  beneficial. 


3  Experimental  Setup 


3.1  General  Description 

The  University  of  Washington  Control  Systems  Laboratory  Two-Link  Arm 
(TLA)  Manipulator  consists  of  a  shoulder  motor,  an  elbow  motor,  elbow  and  wrist 
plastic  air  support  pucks,  an  end  effector  assembly,  and  two  links  connecting  the  mo¬ 
tors  and  end-effector.  The  configuration  is  mounted  to  a  4’  by  8’  granite  table.  Any 
combination  of  rigid  and  flexible  links  is  possible.  In  addition,  it  is  possible  to  run  the 
system  as  a  single  link.  Also,  to  provide  a  low  friction  environment,  both  ends  of  the 
forearm  are  supported  by  pressured  air  coming  out  of  plastic  pucks  at  the  elbow  and 
wrist  [22].  Figure  3.1  shows  a  block  diagram  of  the  TLA  hardware  setup  while  Figure 

3.2  displays  a  picture  of  the  TLA  facility. 


Figure  3.1-  Block  Diagram  of  TLA  Hardware 


3.2  Actuators 

Both  motors  are  brushless  DC  torque  motors  with  effectively  no  backlash.  The 
shoulder  motor  is  an  Aeroflex  TQ82W-1FA  and  the  elbow  motor  is  an  Aeroflex 
TQ40W-11FA.  Each  DC  motor  consists  of  a  stator  and  a  permanent  magnet.  Each 
stator  consists  of  magnetic  conduction  cores  wrapped  by  current  carrying  conductors 
in  a  toroidal  fashion.  The  magnet  acts  as  the  rotor,  and  is  separated  from  the  stator  by 
a  small  air  gap. 

The  brushless  DC  motors  are  simple  2  pole  devices.  The  current  running  through 
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Figure  3.2-  TLA  FacOity  Picture 


the  stator  sets  up  a  magnetic  field  and  the  permanent  magnet  tries  to  align  itself  with 
the  field  under  a  developed  actuator  torque  T  (r)  =  K  (Q)i(t)e.  The  motors  re- 

t 

quire  sourcing  of  the  winding  current  in  either  a  voltage  or  current  control  mode.  For 
ease  of  interface  to  and  isolation  from  the  control  signal,  current  control  mode  is 
used. 

The  power  amplifiers  which  drive  the  motors  are  the  KEPCO  BOP50-8M  for  the 
shoulder  motor  and  the  KEPCO  BOP36-6M  for  the  elbow  motor.  They  are  linear  am¬ 
plifiers  which  can  operate  in  either  voltage  or  current  mode  and  can  sink  100%  of 
their  current  rating  at  any  voltage  setting. 

Since  the  models  used  in  this  research  are  in  terms  of  Newton-meters  and  the  ac¬ 
tual  output  from  the  data  acquisition  system  to  the  actuators  is  in  volts,  a  conversion 
factor  is  required.  Figure  3.3  shows  the  conversion  factors  for  actuator  1  and  2  (the 
shoulder  and  elbow  motors,  respectively). 

3.3  Links 

Four  basic  link  configurations  exist:  SLA_A,  SLA_B,  TLA_A,  and  TLA_B. 
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SLA_A  is  a  single  flexible  link  with  the  tip  bracket  centered  on  the  tip  pad,  while 
SLA_B  is  also  a  single  flexible  link,  but  with  no  tip  load.  Configuration  TLA_A  con¬ 
sists  of  two  flexible  links,  with  the  elbow  stator  tied  to  the  first  link,  and  TLA_B  also 
two  flexible  links,  but  with  the  elbow  stator  tied  to  the  second  link.  In  addition,  a  set 
of  very  flexible  links  has  been  manufactured  for  use  with  the  two  link  configuration, 
but  is  not  used  in  this  research. 


Shoulder  Motor 

Power  Motor  Metric 


Elbow  Motor 

Power 

Voltage  Supply 

From  8A/  I 

AClOO  ^  lOV 


5206  Nm/ 

V _ 


Nm 


Motor  Metric 

Constant  Conversions 


Nm 


V 


n243Nni/| 

V 


Nm 


Figure  3.3-  Actuator  Conversion  Factors 


3.4  Sensors 

The  joint  angle  signals  are  produced  by  optical  encoders  mounted  on  the  DC  mo¬ 
tor  shafts.  Since  the  DC  motors  are  direct  drive,  a  high  degree  of  accuracy  is  available 
from  these  collocated  sensors.  Each  joint  is  fitted  with  a  Hewlett-Packard  HEDS- 
6000  Optical  encoder.  The  encoder  provides  3  channels  of  output,  namely  two  chan¬ 
nels  for  quadrature  square  waves  and  one  channel  for  an  index  pulse.  Both  channels 
A  and  B  provide  1024  cycles  per  revolution.  The  quadrature  lines  and  index  line  are 
fed  directly  to  a  quadrature  decoding  module  in  the  AClOO  data  acquisition  system. 

In  addition,  a  proximity  sensing  system  is  used  to  enable  the  controller  to  know 
when  the  surface  is  close.  Such  knowledge  enables  the  controller  to  slow  the  arm 
down  and  prevent  excessive  speed  and  force  upon  touchdown.  A  Microswitch  MPF5 
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analog  photoelectric  control  is  used  as  the  proximity  sensor.  This  device  is  used  in 
conjunction  with  an  MPS35  sensor  base,  an  MPB 1 1  receptacle,  and  an  FE-B5B-5  fi¬ 
ber  optic  cable  to  provide  an  analog  output  proportional  to  the  straight-line  distance 
from  a  surface.  This  is  a  fiber  optic  system  which  transmits  a  beam  of  infrared  light 
out  of  one  part  of  the  cable  and  receives  the  reflected  light  back  in  the  other  part  of 
the  cable.  The  amount  of  reflected  light  received  back  is  directly  proportional  to  the 
distance  between  the  cable  end  and  a  surface.  The  active  range  of  this  sensor  is  1-4 
inches,  so  by  placing  the  cable  end  1  inch  behind  the  roller  attached  to  the  TLA  wrist, 
the  readout  is  effectively  a  voltage  from  0-5  Volts  which  is  proportional  to  0-3  inches. 
This  particular  sensor  may  be  used  in  further  research  on  the  TLA. 

Also,  a  6-component  force  sensor  mounted  on  the  wrist  provides  force  readings 
in  the  x,  y,  and  z  directions,  as  well  as  moments  about  the  x,  y,  and  z  axis.  The  Fx  and 
Fy  readings  are  used  in  the  force  control  algorithm.  Specifically,  the  sensor  system  is 
an  Assurance  Technologies  Incorporated  Mini  5/10  system,  shown  in  Figure  3.4. 


F/T  Controller 

Figure  3.4-  ATI  Force  Sensor  Block  Diagram 


This  system  includes  a  5  lb/10  in-lb  force/torque  sensor,  and  multiplexor  and  sig¬ 
nal  conditioning  unit,  and  a  controller.  The  controller  reads  in  the  raw  strain  gauge 
voltages,  converts  them  to  a  digital  signal,  applies  a  6x6  calibration  matrix,  and  pro- 
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duces  analog  outputs  directly  proportional  to  the  actual  forces  and  torques.  The 
sensor  is  attached  to  the  end  of  the  second  link  on  one  side  and  to  a  low-friction  roller 
on  the  other  side,  shown  in  Figure  3.5.  This  enables  the  sensor  to  detect  primarily 
only  the  surface  normal.  The  overall  sensor  has  the  capability  shown  in  Figure  3.6. 


Mounting 

Tool  Adapter  Adapter 


Figure  3.5-  Force  Sensing  Mechanism 


Figure  3.6-  Force/Torque  Conventions 
3.5  Data  Acquisition  System 

The  data  acquisition  system  used  for  the  experiments  is  an  ISI  AC100/C30  sys¬ 
tem.  This  system  operates  at  100  Khz  and  interfaces  with  MATRIXx  System  Build 
directly  through  an  autocode  generator.  The  autocode  generator  translates  System 
Build  block  diagrams  into  C  code  which  interfaces  the  hardware  to  the  arm.  The  com¬ 
putational  platform  is  a  Sun  Sparcstation/10. 
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The  AC100/C30  possesses  IP-DIG24,  IP-DAC,  IP-QUAD,  and  IP-HiADC  cards 
which  are  all  installed  on  a  DSPFLEX  board  located  in  a  66  MHz  486  PC.  The  IP- 
DIG-24  provides  24  digital  (TTL)  input/output  channels,  while  the  IP-DAC  provides 
6  12-bit  analog  output  channels  configured  to  +/-5V  or  -lOV.  In  addition,  the  IP- 
QUAD  module  provides  4  quadrature  decoding  channels  which  can  detect  either  po¬ 
sition  or  rate  from  quadrature  signals.  Finally,  the  IP-HiADC  possesses  12  input 
analog  channels  with  12-bit  resolution  and  synchronous  sampling  of  all  inputs. 

3.6  Software  Interface 

MATRIXx  System  Build  is  the  software  interface  for  the  system.  This  system  al¬ 
lows  interface  between  the  controller  block,  the  interaction  block,  and  the  plant 
block.  Each  of  these  blocks  is  a  superblock,  which  contains  individual  functions  dis¬ 
played  as  boxes  inside.  In  this  way,  the  designer  gets  a  clear  picture  of  the  interface 
between  the  individual  components  involved  in  the  design.  Figure  3.7  shows  the 
block  diagram  for  the  overall  simulation  environment. 


24-JAN-9G 
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Figure  3.7-  System  Build  Simulation  Block 

The  interactive  animation  block  allows  the  user  to  change  the  reference  trajectory 
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path  “on-the-fly”.  The  user  may  specify  the  start  angle,  end  angle,  and  slew  time  for 
the  reference  trajectory  while  viewing  the  angle  and  voltage  (torque)  outputs  real¬ 
time.  shows  a  block  diagram  of  the  interactive  animation  block.  Figure  3.8  shows  the 
interactive  animation  block  diagram. 
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Figure  3.8-  Interactive  Animation  Block  Diagram 


The  plant  simulation  block  is  implemented  with  a  state  space  model.  In  addition, 
the  Newton-meter  to  voltage  inverse  conversion  is  needed  to  account  for  this  scaling 
in  the  controller  block.  Finally,  the  angle  output  must  be  converted  from  radians  to 
counts  to  match  the  optical  encoder  and  quadrature  decoder  sensor.  The  block  dia¬ 
gram  is  shown  in  Figure  3.9. 

Lastly,  the  typical  controller  is  implemented  by  the  feedback  structure  shown  in 
Figure  3. 10.  Here,  the  slew  command  is  compared  to  the  actual  angle  and  the  error  is 
fed  into  the  controller.  For  this  setup  (the  SANDY  controller,  to  be  discussed  later), 
the  controller  has  two  inputs  (the  angle  errors)  and  two  outputs  (the  torques).  These 
torques  are  converted  from  Nm  to  Volts  (the  hardware  output  units). 


16 


Appendix  E  contains  listings  of  the  Matlab  m-files  used  to  aid  in  completion  of 
this  dissertation.  In  particular,  the  model  generation,  SANDY  and  LQG  design,  and 
bandwidth  m-files  are  included. 
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Figure  3.9-  Simulation  Plant  Implementation 
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Figure  3.10-  Typical  System  Build  ControDer  Structure 


4  Position  Dynamics  Model 


The  model  used  in  this  research  is  a  lumped-spring-mass  model  based  on  the 
work  of  Oakley  [56].  Appendix  A  provides  the  details  of  the  model,  including  a  com¬ 
plete  6  degree  of  freedom  (DOF)  example  of  the  complete  non-linear  equations  of 
motion  for  a  two-link  manipulator.  In  addition.  Appendix  A  also  gives  the  parameter 
descriptions  and  their  values.  For  a  two-link  arm,  6  DOF  corresponds  to  3  elements 
for  each  link.  The  model  used  for  this  research  assumed  a  20  DOF  model,  or  10  ele¬ 
ments  per  link.  This  corresponds  to  20  velocity  states  and  20  position  states,  or  a  40th 
order  model.  The  model  also  includes  2  additional  outputs  for  x  and  y  end-point  po¬ 
sitions  through  the  use  of  forward  kinematic  equations. 


4.1  Non-Linear  Equations  of  Motion 

The  nonlinear  equations  of  motion  are  developed  by  Oakley  [56]  based  upon 
Kane’s  method  [30].  It  is  convenient  to  express  a  generalized  speed  vector  u  as: 

1,-1 

=  01+  E  (4.1) 

k=\ 

“n,  =  01  +  Z  +  X  ^2k^h  =  1 . «2  (4-2) 

k=\  k=\ 


(4.3) 


where  6i  is  the  shoulder  motor  velocity,  02  is  the  elbow  motor  velocity,  is  a  sub¬ 


link  velocity  for  link  1  and  is  a  sublink  velocity  for  link  2.  Additionally,  the  z  vec¬ 
tor  contains  the  position  of  the  manipulator  and  the  t  vector  contains  the  motor 
torque. 
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(4.4) 


t:  =  (4.5) 

The  complete  non-linear  matrix  equation  for  a  two-link  manipulator  is  therefore 
represented  by  the  following  equation: 

M  {z)  ti  +  V  {z,  u)  +Rz  +  Kz  =  Tx 


(4.6) 


Figure  4.1-  Lumped-Spring-Mass-Damper  Model  Parameters 

where  M  possesses  the  mass  and  inertia  properties,  V  contains  the  centripetal  and  Co¬ 
riolis  terms,  R  represents  the  Coulomb  torsional  damping,  and  K  contains  the  tor¬ 
sional  stiffness.  Also,  T  is  the  torque  input  distribution  matrix. 


4.2  Linearized  Equations  of  Motion 

Linearization  may  occur  about  any  joint  condition  with  zero  link  deflections  and 
all  velocity  states  equal  to  zero.  Application  of  a  Taylor’s  series  expansion  about  the 
non-linear  equations  of  motion  in  equation  (4.6)  produces  the  desired  linearization. 
The  resulting  linearized  equation  of  motion  is; 

+  ifAz  =  JAt  (4.7) 

where  M  is  a  Taylor  expansion  of  M  about  the  chosen  linearization  condition.  V  from 
the  non-linear  equation  disappears  since  it  is  a  function  of  velocity  squared,  and  ve¬ 
locity  is  assumed  small  near  the  linearization  state. 

It  is  convenient  to  place  the  linearized  equations  of  motion  in  the  state  space  form 
of  the  following  equations. 
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(4.11) 

(4.12) 

(4.13) 

(4.14) 


Matlab  m-files  tlapar.m  and  tlagen.m  achieve  the  linearization  using  this  scheme.  Al¬ 
so,  the  m-files  are  converted  to  MATRIXx  form  in  tlapar.mws  and  tlagen.mws  to 
achieve  the  linearization.  The  tlapar.m  file  contains  the  parameters  which  are  specific 
to  the  manipulator  model,  such  as  the  hub  inertia,  as  well  as  containing  the  number 
of  elements  per  link  desired.  After  loading  the  parameters,  running  the  tlagen.m  file 
actually  generates  the  new  A  and  B  matrices,  and  calls  them  Abar  and  Bbar.  The  input 
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to  the  model  is  the  torque  vector  and  the  output  contains  the  states  as  well  as  x  and  y 
endpoint  positions.  The  original  work  for  the  m-files  was  done  by  Evers  [22]. 


4.3  End-point  Calculations 

The  end-point  calculations  are  not  explicitly  part  of  the  equations  of  motion. 
However,  the  x  and  y  end-point  positions  are  provided  as  the  last  2  outputs  from  the 
model  for  use  in  simulation  and  control.  The  equations  are: 
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4.4  Low-Torque  Model 

Model  parameter  identification  was  achieved  through  applying  3  different  slew 
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trajectories  to  2  different  Proportional-Integral-Derivative  (PID)  controllers,  and 
then  choosing  the  damping  and  flexural  rigidity  terms  to  best  match  the  experimental 
response.  Table  4.1  shows  the  PID  controller  gains  used  to  perform  parameter  iden¬ 
tification  for  the  low-torque  model.  When  the  applied  torque  onset  is  relatively  slow, 
friction  effects  tend  to  increase  the  damping  and  lower  the  natural  frequency. 


Table  4.1  -  PID  Parameter  ID  gains 


Controller 

P 

I 

1 

.5 

2 

.5 

.01 

_ £1 _ 1 

Different  controllers  and  trajectories  were  used  to  ensure  the  model  was  not  tuned 
to  one  controller  or  trajectory.  A  relatively  high  PID  gain  was  used  for  controller  1  to 
provide  many  overshoots  for  ease  of  damping  identification,  while  a  lower  gain  was 
used  for  controller  2  to  provide  insight  into  bearing  stiction  effects  at  lower  gains. 
The  3  test  slews  were  chosen  to  excite  dynamics  for  moving  each  joint  individually 
while  holding  the  other  constant  as  well  as  checking  the  dynamics  for  slewing  both 
links  at  the  same  time.  Table  4.2  displays  the  3  test  slews  used  for  parameter  ID  and 
Table  4.3  contains  the  resulting  values  of  these  tuned  parameters. 


Table  4.2  -  Test  Slews  for  Parameter  ID 


Slew 

01_start  (deg) 

91_end  (deg) 

02_start  (deg) 

02_end  (deg) 

1 

40 

0 

% 

2 

0 

0 

110 

70 

3 

20 

0 

no 

90 

Table  4.3  -  Experimentally 

TVined  Values 

for  the  Low-Torque  Model 

Link 

b 

El 

1 

- IB - 

2 

.1 

.3 

These  tuned  values  provide  a  model  with  eigenvalues  listed  in  Table  4.4. 


Table  4.4  -  Low-Torque  Model  Eigenvalues 


High 

Torque 

Eig 

High 

Torque 

Damping 

High 

Torque 

Freq 

(rad/s) 

0 

1 

0 

0 

1 

0 

0 

1 

0 

0 

1 

0 

5.1174e-01 

1 

5.1174e-01 

5.1174e-01 

1 

5.1174e-01 

5.1174e-01 

1 

5.1174e-01 

5.1174e-01 

1 

5.1174e-01 

5.1174e-01 

1 

5.1174e-01 

5.1175e-01 

1 

5.1175e-01 

5.1175e-01 

1 

5.1175e-01 

5.1187e-01 

1 

5.1187e-01 

5.1279e-01 

1 

5.1279e-01 

-2.8198e+00+  1.7317e+01i 

0.161 

1.7545e+01 

-2.8198e+00-  1.7317e+01i 

0.161 

1.7545e+01 

-2.6450e+01+  4.6776e+01i 

0.492 

5.3736e+01 

-2.6450e+01-  4.6776e+01i 

0.492 

5.3736e+01 

-5.4610e+01 

1 

-5.4610e+01 

-5.4621e+01 

1 

-5.462  le+01 

-5.4650e+01 

1 

-5.4650e+01 

-5.4724e+01 

1 

-5.4724e+01 

-5.4952e+01 

1 

-5.4952e+01 

-5.5897e+01 

1 

-5.5897e+01 

-6.3302e+01 

1 

-6.3302e+01 

-2.4982e+02 

1 

-2.4982e+02 

-3.9645e+02 

1 

-3.9645e+02 

-2.1615e+03 

1 

-2.1615e+03 

-2.3269e+03 

1 

-2.3269e+03 

-8.1917e+03 

1 

-8.1917e+03 

-2.1660e+04 

1 

-2.1660e+04 

-2.5780e+04 

1 

-2.5780e+04 
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Table  4.4  -  Low-Torque  Model  Eigenvalues 


High 

Torque 

Eig 

High 

Torque 

Damping 

High 

Torque 

Freq 

(rad/s) 

-4.6849e+04 

1 

-4.6849e+04 

-8.4972e+04 

1 

-8.4972e+04 

-1.2623e+05 

1 

-1.2623e+05 

-1.6286e+05 

1 

-1.6286e+05 

-6.1081e+05 

1 

-6.1081e+05 

-1.6673e+06 

1 

-1.6673e+06 

-3.6715e+06 

1 

-3.6715e+06 

-6.7348e+06 

1 

-6.7348e+06 

-1.0076e+07 

1 

-1.0076e+07 

The  Bode  plot  for  Gj/Tj  (theta_l  to  torque_l)  is  shown  in  Figure  4.2,  while  the 
Bode  plot  for  02/t2  is  seen  in  Figure  4.3. 
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Figure  4.2-  Low-Torque  Model  Bode  Plot,  Gj/Xi 
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Figure  4.3-  Low-Torque  Model  Bode  Plot,  02/T2 
4.5  High-Torque  Model 

When  a  higher  initial  torque  onset  is  applied  to  the  motors,  the  friction  effects  are 
less  noticeable.  The  model  is  matched  to  the  SANDY  controller  developed  in  Chap¬ 
ter  7,  and  new  b  and  El  values  are  obtained.  Table  4.3  shows  these  new  high-torque 
values. 

Table  4.5  -  Experimentally  Ihned  Values  for  the  High-Torque  Model 


Link 

b 

El 

1 

.u6 

5.5 

2 

.03 

5.5 

Applying  the  new  values  results  in  a  model  with  eigenvalues  shown  in  Table  4.6. 
This  new  model  has  a  Bode  plot  for  Gj/tj  as  shown  in  Figure  4.4.  Also,  the  Bode  plot 
for  ©2/^2  is  shown  in  Figure  4.5. 

4.6  Model  Reduction 

Having  a  40th  order  model  results  in  large  computational  burdens  for  simulation 
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and  the  design  process.  Also,  since  observer-based  output  feedback  controllers  have 
the  same  order  as  the  order  of  the  model,  it  is  desirable  to  decrease  the  order  of  the 


Table  4.6  -  High-Torque  Model  Eigenvalues 
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Figure  4.4-  High-Torque  Model  Bode  Plot,  Bj/Ti 


.1  1  io 

Frequency  [rad/secl 
High  Torque,  Theta„2-Torque_2 


too 


Figure  4.5-  High-Torque  Model  Bode  Plot,  02/X2 


plant  model  as  much  as  possible  without  losing  much  dynamic  information.  One  way 
to  reduce  the  model  order  is  to  balance  the  system  using  a  balanced  realization  algo¬ 
rithm  and  removing  the  non-dominant  modes  (those  with  low  Hankel  singular  val¬ 
ues).  This  was  accomplished  by  use  of  the  MATLAB  balsq.m  command  which 
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balances  and  trancates  the  system.  In  order  to  perform  the  balancing  on  a  system 
which  is  not  asymptotically  stable,  a  special  algorithm  is  needed  which  first  removed 
the  4  rigid  body  poles  at  the  origin,  balanced  the  remaining  part  of  the  system,  and 
then  reattached  the  rigid  body  modes. 

By  applying  the  balanced  truncation  method,  the  plant  was  reduced  to  18th  order 
for  the  high-torque  model.  Figure  4.6  and  Figure  4.7  display  the  Bode  plots  for  the 
reduced  model  for  the  high-torque  model.  Notice  the  reduced  model  matches  the  full- 
order  models  well  below  100  rad/s,  which  is  the  primary  area  of  concern  for  this  flex¬ 
ible  manipulator.  Table  4.8  shows  the  eigenvalues  for  the  reduced  system.  Note  the 
close  match  of  the  eigenvalues  between  the  reduced-order  model  and  the  full-order 
model  eigenvalues. 


Table  4.7  -  18th  Order  Reduced  High-Torque  Model  Eigenvalues 


Eigenvalues 

Damping 

Frequency 

(rad/s) 

-8.6736e-18-  6.9728e-10i 

1.2439e-08 

6.9728e-10 

-1.5408e-14+  2.0437e-08i 

7.5392e-07 

2.0437e-08 

-1.5408e-14-  2.0437e-08i 

7.5392e-07 

2.0437e-08 

-8.4597e-01+  7.5120e+01i 

1.1261e-02 

7.5125e+01 

-8.4597e-01-  7.5120e-i-01i 

1.1261e-02 

-9.3877e-01+  5.5952e+01i 

1.6776e-02 

5.5960e+01 

-9.3877e-01-  5.5952e-l-01i 

1.6776e-02 

5.5960e-l-01 

-7.9390e+00-i-  2.2995e-t-02i 

3.4505e-02 

2.3009e-i-02 

-7.9390e+00-  2.2995e+02i 

3.4505e-02 

2.3009e+02 

-8.1176e+00-t-  1.6425e-t-02i 

4.9361e-02 

1.6445e-i-02 

-8.1176e+00-  1.6425e+02i 

4.9361e-02 

-7.0065e-(-01+  6.7562e+02i 

1.0315e-01 

6.7925e+02 

-7.0065e-h01-  6.7562e-t02i 

1.0315e-01 

6.7925e-i-02 

-1.0330e+02+  5.6669e+02i 

1.7933e-01 

5.7603e+02 

-I.0330e+02-  5.6669e-h02i 

1.7933e-01 

5.7603e-l-02 

-4.4510e-i-02+  1.6607e-i-03i 

2.5889e-01 

1.7193e+03 

-4.4510e-H02-  1.6607e-h03i 

2.5889e-01 

1.7193e-H03 

Similarly,  balanced  truncation  was  applied  to  the  low-torque  model.  In  this  case, 
only  a  33rd  order  reduced  model  was  obtained  successfully.  Figure  4.8  and  Figure 
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4.9  show  the  Bode  plots  for  the  reduced  order  system.  Notice  that  it  matches  the  full- 
order  system  well.  Table  4.8  shows  the  eigenvalues  for  the  reduced  33rd  order 
system. 


Table  4.8  -  33rd  Order  Reduced  Low-Torque  Model  Eigenvalues 


Eigenvalues 

Damping 

Frequency 

(rad/s) 

8.45U4e-U8 

-1 

8.45U4e-08 

2.1235e-09 

-1 

2.1235e-09 

-2.1235e-09 

1 

2.1323e-09 

-8.4516e-08 

1 

8.4516e-08 

-5.1186e-01 

1 

5.1186e-01 

-5.1301e-01 

1 

5.1301e-01 

-9.3523e-01 

1 

9.3523e-01 

-2.8207e+00+  1.7319e+01i 

1.6075e-01 

1.7547e+01 

-2.8207e+00-  1.7319e+01i 

1.6075e-01 

1.7547e+01 

-2.6465e+01+  4.6794e+01i 

4.9229e-01 

5.3760e+01 

-2.6465e+01-  4.6794e+01i 

4.9229e-01 

5.3760e+01 

-5.4620e+01 

1 

5.4620e+01 

-5.4620e+01+  6.5820e-01i 

9.9993e-01 

5.4624e+01 

-5.4620e+01-  6.5820e-01i 

9.9993e-01 

5.4624e+01 

-5.4648e+01 

1 

5.4648e+01 

-5.4868e+01 

1 

5.4648e-t-01 

-5.5914e+01 

1 

5.5914e+01 

-6.4390e+01 

1 

6.4390e+01 

-2.4392e+02 

1 

2.4392e+02 

-3.9608e+02 

1 

3.9608e+02 

-2.061  le+03 

1 

2.061  le+03 

-2.3130e+03 

1 

2.3130e+03 

-8.3537e+03 

1 

8.3537e+03 

-2.2131e+04 

1 

2.213  le+04 

-3.2986e+04 

1 

3.2986e+04 

-4.6661e+04 

1 

4.666 le+04 

-8.4488e+04 

1 

8.4488e+04 

-1.2356e+05 

1 

1.2356e+05 

-4.1591e+05+7.7674e+04i 

9.8300e-01 

4.2310e+05 

-4.1591e+05-  7.7674e+04i 

9.8300e-01 

4.2310e+05 

-2.0474e+06 

1 

2.0474e+06 

-4.1778e+06 

1 

4.1778e+06 

-6.7552e+06 

1 

6.7552e+06 

Phase  [Deg]  Gain  [dB] 


Frequency  [rad/sec] 

High  Torque,  18th  Order,  Theta_1-Torque_1 

Figure  4.6-  High-Torque  Model  Bode  Plot,  01/xl  of  the  Reduced-Order  Model 


Frequency  [rad/sec] 

High  Torque,  18th  Order,  Theta_2-Torque_2 


Figure  4.7-  High-Torque  Model  Bode  Plot,  02/t2  of  the  Reduced-Order  Model 
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4.7  Experimental  Model  Validation 

One  way  to  determine  the  validity  of  a  model  is  to  conduct  a  frequency  response 
of  the  plant  and  compare  the  experimental  results  to  the  mathematical  model  frequen- 


Figure  4.8-  Low  -Torque  Model  Bode  Plot,  01/t1  of  the  Reduced-Order  Model 


Figure  4.9-  Low  -Torque  Model  Bode  Plot,  92/t2  of  the  Reduced-Order  Model 
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cy  response.  Since  most  of  the  design  work  involves  the  high-torque  model,  only  it 
will  be  compared  to  the  experimental  data  directly. 

In  order  to  acquire  the  frequency  data  experimentally,  a  sine  wave  voltage  is  fed 
directly  into  one  of  the  motors.  Ideally,  no  torque  should  be  applied  to  the  second  mo¬ 
tor  for  the  most  accurate  representation  of  the  open-loop  model.  As  a  matter  of 
practice,  though,  a  small  proportional  gain  is  required  to  hold  the  second  link  in  place 
during  sine  sweeps  of  the  first.  For  these  small  proportional  gains,  the  resulting  ex¬ 
perimental  model  is  close  to  the  open-loop  model  [56].  Specifically,  a  proportional 
gain  of  0.5  is  used  to  close  the  loop  of  the  motor  which  is  not  sinusoidally  excited. 

To  experimentally  implement  the  sine  sweep  for  the  second  link,  the  setup  of  Fig¬ 
ure  4.10  is  used.  This  feeds  a  sinusoidal  voltage  into  one  motor  while  keeping  the 
other  motor  at  a  specified  reference  position.  To  conduct  a  sine  sweep  for  the  first 
link,  just  reverse  the  reference  and  sinusoid  of  Figure  4. 10. 


Amplitude 


Figure  4.10-  Experimental  Sine  Sweep  Block  Diagram  (Link  2) 

The  raw  data  is  taken  as  volts  (the  input)  and  theta  (the  output).  Of  course,  the 
actual  units  required  are  Newton-meters  (Nm)  of  torque  for  input.  Thus,  knowing  the 
conversion  from  Nm  to  volts  for  each  motor  and  the  amplitude  of  the  response  in  ra¬ 
dians,  the  transfer  function  of  degrees/Nm  for  amplitude  is  obtained.  By  knowing  the 
frequency  and  the  delay  in  seconds  between  the  input  and  output,  the  phase  shift  in 
degrees  is  obtained.  Figure  4.11  shows  the  experimental  vs.  model  frequency  re- 
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4.8  Mode  Shapes 

Each  of  the  peaks  on  the  Bode  plot  correspond  to  flexible  modes  of  the  arm.  The 
modes  alternate  between  the  first  and  second  link,  starting  with  mode  1  on  the  first 
link.  Thus  mode  2  is  actually  the  first  mode  of  the  second  link.  The  mode  shape  can 
be  determined  by  the  number  of  nodes  it  has,  with  the  number  of  nodes  equal  to  the 
link  mode  minus  one.  Thus,  the  first  and  second  modes  are  essentially  first  modes  so 
they  have  no  nodes.  The  third  mode  is  the  second  mode  for  the  first  link,  and  there¬ 
fore  has  one  node. 

For  the  semi-flexible  links  used  in  this  research,  the  mode  deflections  are  rela¬ 
tively  small  and  therefore  hard  to  see  in  a  single  camera  shot.  However,  careful 
examination  of  the  pictures  can  show  some  of  the  mode  shape.  First,  Figure  4.15 
shows  mode  1,  which  has  no  nodes  and  is  on  the  1st  link.  There  is  a  slight  deviation 
to  the  right  of  the  centerline  in  this  picture. 


Figure  4.15-  1st  Mode,  Semi-Flexible  Links  (Link  1,  Mode  1) 


The  second  mode  can  be  seen  in  Figure  4. 16.  This  is  the  first  mode  for  the  second 
link  and  thus  has  no  nodes.  There  is  a  slight  deviation  to  the  left  here,  and  clearly  no 
nodes. 


Figure  4.16-  2nd  Mode,  Semi-Flexible  Links  (Link  2,  Mode  1) 

Finally,  the  third  mode  is  shown  in  Figure  4.17.  Here,  a  node  appears  roughly 
the  eenter  of  the  first  link. 


Figure  4.17-  3rd  Mode,  Semi-Flexible  Links  (Link  1,  Mode  2) 


5  PID  Design  and  Experiments 


In  order  to  establish  a  standard  for  comparison  and  to  experimentally  validate  the 
system,  simple  baseline  experiments  are  performed.  In  particular,  a  basic  proportion¬ 
al-integral-derivative  (PID)  controller  is  simulated  and  experimentally  tested  on  the 
two-link  arm.  Additional  plots  are  shown  in  Appendix  B  for  different  test  trajectories 
and  verify  that  the  results  for  this  trajectory  are  valid  for  others  as  well. 

5.1  PID  Design 

Logically,  the  first  control  law  implemented  on  a  new  system  should  be  as  simple 
as  possible.  Therefore,  initial  testing  is  done  with  a  simple  proportional-integral-de¬ 
rivative  (PID)  controller  [17].  It  is  represented  with  equation  (5.1). 

t(5)  =  +  +  (5.1) 

To  achieve  better  performance,  the  PID  controller  was  tuned  iteratively  with 
hardware  runs  to  achieve  good  performance  in  trajectory  tracking.  Table  5.1  lists  the 
high  performance  PID  gains. 


Table  5.1  -  High  Performance  PID  Gains 


Link 

Kp 

Ki 

Kd 

1 

1 

.5 

2 

2 

1 

.5 

2 

This  PID  controller  was  implemented  on  the  AC  100  system  with  a  sampling  time 
of  1  ms.  Figure  5.1  shows  a  diagram  of  the  test  trajectories  listed  in  Table  5.2. 


Figure  5.1-  TLA  Test  Trajectories 


theta_2_ref  (rads)  theta_1_ref  (rads) 
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Figure  5.2  shows  the  reference  commands  for  trajectory  2  while  Figure  5.3  shows  the 
angle  conventions. 


Reference  Commands  for  Trajectory  2 

Figure  5.2-  Trajectory  2  Reference  Commands 


Figure  5.3-  Angle  Conventions 


Table  5.2-  Test  Slews  for  Parameter  ID 


Trajectory 

01  start 
(deg) 

01_end 

(deg) 

02_start 
(deg) . 

02_end 

(deg) 

1 

40 

0 

90 

""  % 

2 

20 

0 

110 

90 

3 

0 

0 

110 

70 
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Figure  5.4  shows  the  experimental  results  for  trajectory  2.  Figure  5.5  and  Figure 
5.6  show  experimental  versus  simulation  results  for  trajectory  2.  Please  note  that  all 
the  plots  in  this  report  have  time  units  of  seconds  and  have  angle  outputs  relative  to 
the  starting  position.  Notice  the  reasonably  close  match  between  experimental  and 
simulation  for  both  angles. 


0  2  4  6  8  10  12  14  16  18  20 

t 

PID=1,.5,2,Traj2 

Figure  5.4-  PID  Experimental  Results,  Trajectory  2 


PIDxsl,.5.2.  Exp  VS  Sinn,  TraJ  2 


Figure  5.5-  PID  Experimental  vs.  Simulation,  Trajectory  2, 0i 


Phase  [Deg]  Gain  [dB] 


Figure  5.6-  PID  Experimental  vs.  Simulation,  Trajectory  2, 02 
5.2  PID  Control  Bandwidth 

Analyzing  the  closed-loop  controller  bandwidth  gives  insight  into  the  physical 
controller  performance.  Figure  5.7  shows  the  0i/9icmd  control  bandwidth. 


.1  1  10  100 

Frequency  [rad/sec] 

PID  BW,  Th_1-Th_1cmd 


Figure  5.7-  PID  Bi/Gicmd  Control  Bandwidth 
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Figure  5.8  shows  the  02/02cmd  control  bandwidth.  The  convention  for  this  re¬ 
search  is  to  define  the  bandwidth  where  the  magnitude  first  crosses  the  -3  dB 
magnitude.  Notice  the  initial  bandwidth  for  0icmd  is  about  2  rad/s,  but  has  a  peak  res¬ 
onance  at  about  70  rad/s.  Likewise,  the  second  loop  has  an  initial  bandwidth  of  about 
9  rad/s,  but  has  a  resonant  peak  at  about  90  rad/s. 


PID  BW,  Th_2-Th_2cmd 

Figure  5.8-  PID  02/02cmd  Control  Bandwidth 


6  LQG-CIass  Controllers  Design  and  Experiments 

As  PID  is  the  baseline  for  classical  controllers,  Linear-Quadratic-Gaussian 
(LQG)  is  the  baseline  for  modern  (or  multiple-input,  multiple-output)  controllers. 
This  chapter  investigates  various  designs  for  LQG-class  position  controllers,  and  ex¬ 
perimentally  evaluates  them.  Since  the  ultimate  goal  of  this  dissertation  is  to  develop 
reduced-order  controllers,  it  makes  sense  to  evaluate  LQG-class  controllers,  which 
are  full-order  controllers. 

First,  a  standard  Linear-Quadratic-Gaussian  (LQG)  controller  is  designed  and 
tested,  and  then  is  extended  with  Loop  Transfer  Recovery  (LQG/LTR)  controller  de¬ 
sign.  Then,  Loop  Transfer  Recovery  based  on  Special  Coordinate  Basis  (SCB/LTR) 
analysis  of  the  plant  is  covered.  In  order  to  make  the  LQG/LTR  and  LTR  controllers 
more  practical,  controller  reduction  is  applied  using  modal  residualization  and  bal¬ 
anced  truncation. 

6.1  LQG  Controller 

As  a  baseline,  a  Linear-Quadratic-Gaussian  (LQG)  controller  is  developed.  An 
LQG  controller  involves  a  Linear-Quadratic-Regulator  (LQR)  for  full-state  feedback 
gains,  and  a  Linear-Quadratic-Estimator  (LQE)  to  estimate  the  states  from  measured 
outputs.  The  resulting  controller  is  the  same  order  as  the  plant. 

6.1.1  LQR  Background 

The  details  behind  LQG  control  are  well-documented  [1]  [35].  First,  a  Linear- 
Quadratic-Regulator  (LQR)  design  determines  the  performance  based  on  full-state 
feedback.  The  objective  of  an  LQR  design  is  to  minimize  a  cost  function  through  the 
full-state  feedback  control  u(t).  Given  a  state  space  system  of  the  form 

x{t)  =Ax(t)+Bu(t)  (6.1) 

The  LQR  cost  function  is: 

(r) -i-m^(0Rm  (r)  jjr  (6.2) 

where 


x(t)  is  the  state  vector 
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-  u(t)  is  iJie  control  vector 

-  Q  is  the  penalty  on  the  states 

-  R  is  the  penalty  on  the  control 

The  control  which  minimizes  the  cost  function  is 

u{t)  =  -R^B^Sx{t)  (6.3) 

where  S  is  the  solution  to  the  steady  state  Algebraic  Riccati  Equation: 

0  =  -  A^S  -SA  +  SBR~^B^S  -  Q  (6.4) 

Often,  the  optimal  gain  G  is  represented  as 

G  =  R~^B^S  (6.5) 

It  is  also  possible  to  penalize  criterion  variables  in  the  cost  function  instead  of  just 

the  states,  even  though  all  the  states  are  still  required  for  full-state  feedback.  In  this 

case  the  criterion  vector  yj.(t)  replaces  the  state  vector  x(t)  in  the  cost  function  of 

Equation  (6.2). 

6.1.2  LQR  Application 

LQR  provides  the  performance  aspect  of  the  LQG  controller,  so  an  LQR  control¬ 
ler  must  be  designed  first.  For  the  TLA,  it  is  desirable  to  have  an  integral  state  in  the 
controller  for  each  control  loop  to  reduce  steady-state  error.  Thus,  the  18th  order 
model  is  augmented  with  two  integral  states  to  produce  a  20th  order  model.  The  cri¬ 
terion  variables  are  the  integral  errors  for  joint  1  and  joint  2,  as  well  as  Oj  and  02. 
After  several  iterations,  the  following  were  the  values  chosen  for  Q  and  R: 

0.001  0  0  0 

Q  ^  0  0.002  0  0 

0  0  50  0 

_  0  0  0  70_ 

Of  course,  since  the  states  in  the  reduced  model  have  no  direct  physical  meaning, 
full-state  feedback  cannot  be  applied  to  the  arm.  Thus,  some  method  of  state  estima¬ 
tion  is  needed  to  implement  this  controller.  The  optimal  gain  G  produced  from  these 
values  is  found  in  Table  6. 1 .  This  gain  can  be  further  divided  into  the  estimator  gains, 
Ge,  and  the  integral  gains,  Gi.  The  estimator  gains  correspond  to  the  first  18  (non- 


0.001  0 
0  0.0001 


(6.6) 
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integral)  states,  and  the  integral  gains  correspond  to  the  last  2  (integral)  states. 


Table  6.1  -  LQR  Optimal  Gain  G 


State 

Torque_l 

Torque_2 

xi 

-1.26y4e+03 

l.l9')4e+03 

X2 

1.1818e+03 

1.1796e+02 

X3 

9.2732e+00 

9.0177e+01 

X4 

5.8135e+00 

-2.6422e-l-00 

X5 

1.5482e+00 

4.0738e+00 

X6 

7.5760e+00 

1.3725e+01 

X? 

-2.9797e-01 

-9.6501e+00 

Xg 

-5.0249e-01 

3.0444e+00 

X9 

-1.3418e+00 

1.5026e+01 

XlO 

-1.2585e+00 

-5.1786e+00 

Xll 

-5.0386e-02 

1.5479e+00 

Xl2 

-8.7266e+00 

7.9195e+00 

^13 

-2.4347e-01 

-6.8467e+00 

Xi4 

2.0838e-02 

-3.0895e+00 

Xl5 

-3.0416e-02 

-1.1861e+00 

Xl6 

8.9189e-02 

2.1206e+00 

Xl7 

5.4804e-01 

2.9006e-01 

Xl8 

1.1500e-01 

-9.4008e-01 

Xi9 

-2.2360e+02 

1.5767e+00 

X20 

-1.8656e+00 

-2.6457e+02 

6.1.3  LQE  Background 

The  Linear-Quadratic-Estimator  (LQE)  allows  implementation  of  the  LQR  full- 
state  feedback  gains  by  estimating  the  states  from  the  sensor  outputs.  Given  the  fol¬ 
lowing  system 


X  (t)  =  Ax  (t)  +  Bu  (t) +Tw  (t)  (6.7) 

y{t)  =  Cx{t)  +Du{t)  +v(t)  (6.8) 

where 

-  w(t)  is  a  process  noise  vector,  E[w(t)]=0,  E[w(t+T)w^(t)]=Wo6(T) 

-  v(t)  is  a  sensor  noise  vector,  E[v(t)]=0,  E[v(t+T)v^(t)]=Vo5(x) 


The  problem  of  optimal  linear  state  estimation  is  to  find  the  best  estimate  for  the 


states  Jr  (r)  such  that 
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/=  lim£[{x(0-A:(0}^e{j^(0-^(0}]  (6.9) 

f  oo 

The  optimal  estimator  gain  matrix  K  which  minimizes  Equation  (6.9)  is 

K  =  (6.10) 

where  is  the  optimal  state  error  covariance  which  is  the  solution  to  the  steady- 
state  Algebraic  Riccati  Equation 

AX^  +  -  X^C^V~^CX^  +  TWy  =0  (6. 1 1) 

6.1.4  LQE  Application 

Since  the  two  integral  states  are  part  of  the  control  law,  they  shouldn’t  be  estimat¬ 
ed.  Therefore,  only  the  other  18  states  need  to  be  estimated.  One  drawback  of 
applying  LQE  is  having  to  define  the  process  and  sensor  noise  characteristics.  In  this 
case,  it  was  decided  to  have  no  true  process  noise  and  assume  the  noise  was  in  the 
sensors.  To  put  some  weighting  on  the  plant,  a  low  value  of  process  noise  was  applied 
to  the  control  inputs  (like  LQG/LTR).  The  LQG  controller  was  designed  with  the  fol¬ 
lowing  values: 


0.01 

0 

’^0  = 

1  0 

_  0 

0.01_ 

_0  1_ 

These  values  produce  the  K  gains  listed  in  Table  6.2.  The  simulation  between  the 
LQR  and  LQG  controllers  using  this  K  are  exactly  the  same,  since  the  initial  estimate 
of  the  states  in  the  simulation  is  the  same  as  the  actual  states. 


Table  6.2  -  LQE  Optimal  K  (LQG) 


Estimated 

yi 

y2 

State 

Xl 

X2 

2.6009e-02 

-3.7294e-02 

X3 

-1.1949e-01 

8.5349e-01 

M 

-3.6048e-01 

1.0828e+00 

5.0741e-02 

6.8298e-02 

X6 

-2.3098e-03 

1.5583e-03 

4.9844e-04 

1.0644e-03 

45 


Table  6.2  -  LQE  Optimal  K  (LQG) 


Estimated 

State 

yi 

y2 

xg 

X9 

7.7974e-04 

2.5519e-03 

xio 

-8.4166e-{)4 

6.4299e-03 

Xll 

4.4648e-03 

Xl2 

-2.0622e-04 

Xl3 

1.6271e-05 

■HeistaiCTBii 

Xi4 

2.1410e-05 

3.6260e-04 

Xl5 

-2.8443e-07 

-2.2607e-06 

Xl6 

-8.3606e-06 

-1.2918e-05 

Xl7 

-1.7028e-05 

-1.9769e-06 

Xl8 

-1.8682e-05 

2.0152e-06 

The  LQG  controller  is  formed  by  recombining  the  estimated  states  with  the  two 
integral  states.  By  defining  Ae=A(l;18,l:18),  Be=B(l:  18,1:2)  and  Ce=C(3:4,l:18) 
to  correspond  to  the  18  non-iiitegral  states,  2  torque  inputs,  and  2  integral  error  out¬ 
puts,  then  the  controllers  are  formed  with  Equations  (6.13)  -  (6.16). 


Acont  = 


{Ae-KCe-BeGe)  -BeGi 
0  0 


(6.13) 


Bcont  = 


-H,I 


(6.14) 


Ccont  =  -G  (6.15) 

Dcont  =  02xA  (6.16) 

where  H  is  the  integral  selection  matrix  and  I  is  a  2x2  identity  matrix.  In  this  case,  H 


is  represented  by  Equation  (6. 17). 


H  = 


1  0 
0  1 


(6.17) 


6.1.5  LQG  Implementation 

The  experimental  results  of  all  the  controllers  are  evaluated  on  one  or  more  of  the 
three  trajectories  mentioned  in  Chapter  5.  These  slews  are  fed  in  as  command  refer¬ 
ences  through  a  quintic  spline  for  each  angle.  A  picture  of  the  slews  and  command 
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trajectories  are  shown  in  Chapter  5. 

The  controllers  are  implemented  as  digital  controllers  at  a  1  ms  sampling  time 
through  a  Tustin  transformation  [28].  The  initial  assumption  that  digital  effects 
would  be  small  at  a  1  ms  sampling  time  was  validated  through  simulation  and  exper¬ 
imentation.  Figure  6. 1  shows  the  experimental  data  for  the  LQG  controller  run  at  a  1 
ms  sampling  time  along  trajectory  2.  Figure  6.2  and  Figure  6.3  show  experimental 
vs.  simulation  for  the  same  trajectory.  Please  note  that  all  plots  in  this  chapter  have 
time  units  of  seconds  and  have  angle  outputs  measured  relative  to  the  starting 
position 


Figure  6.1-  LQG  Controller  Experimental  Run  -  Traj  2 


6.2  LQG/LTR 

Although  LQG  implements  an  LQR  design  and  is  guaranteed  to  be  stable,  it  has 
no  guaranteed  robustness.  An  approach  was  developed  by  Doyle  [20]  to  recover  ro¬ 
bustness  asymptotically.  This  approach  involves  adding  process  noise  to  the  control 
inputs  and  enables  LQR  robustness  to  be  recovered  if  the  system  is  left-invertible  and 
minimum-phase  as  the  noise  is  increased  to  infinity.  As  a  practical  limit,  the  designer 
can’t  specify  infinite  noise  because  the  control  gains  become  too  large. 


LQG  Exp  vs.  Sim,  Traj  2 


Figure  6.3-  LQG,  Exp  vs.  Sim,  Traj  2, 02 

For  this  application,  when  LTR  process  noise  weightings  were  much  above  20,  the 
arm  would  start  to  vibrate  due  to  numerical  roundoff  of  the  finite  single-precision 
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word  length  in  the  data  acquisition  system. 

The  following  weightings  produced  an  acceptable  LQG/LTR  design: 


(6.18) 


20  0 

= 

1  0 

_0  20_ 

_0  1_ 

Table  6.2  shows  the  resulting  LQG/LTR  estimator  gain  matrix  K.  Notice  that  the  ef¬ 
fect  of  the  LQG/LTR  in  comparison  to  the  LQG  is  to  provide  higher  gains  in  K,  and 
thus  higher  gains  in  the  controller. 


Table  6.3  -  LQG/LTR  Optimal  K  (LQG) 


Estimated 

State 

yl 

y2 

Xl 

X2 

4.8338e-01 

-2.9498e+00 

^3 

3.8446e+01 

X4 

-1.4864e+01 

4.8814e+01 

^5 

2.9153e+01 

3.9547e+01 

X6 

1.8499e+00 

9.8563e+00 

^7 

9.5589e-01 

-2.9224e+00 

Xg 

4.1687e-01 

2.8492e+01 

X9 

1.2974e+00 

5.1211e+00 

XlO 

-1.56336+00 

1.2459e+01 

^11 

-1.3231e+01 

8.3106e+00 

Xl2 

-1.4522e+00 

-8.3386e-03 

Xl3 

9.4262e-02 

5.9582e-02 

Xi4 

5.8507e-02 

6.4868e-01 

Xl5 

1.0669e-02 

4.2726e-02 

Xl6 

-2.8636e-02 

-8.4883e-03 

^17 

-8.7449e-02 

-5.9608e-02 

Xl8 

1.9548e-02 

6.6394e-02 

The  experimental  results  for  the  LQG/LTR  controller  are  shown  in  Figure  6.4  for  tra¬ 
jectory  2.  The  results  are  very  close  to  the  original  LQG  controller. 


6.3  Special  Coordinate  Basis  Loop  Transfer  Recovery  (SCB/LTR) 

Another  way  to  recover  robustness  makes  use  of  special  coordinate  basis  (SCB) 
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analysis.  SCB  analysis  is  a  method  for  examining  system  characteristics  such  as  in- 
vertibility  and  invariant  zeros  directly  from  the  SCB  structure  [64].  The  Loop 
Transfer  Recovery  uses  the  SCB  form  to  minimize  the  difference  between  the  ob¬ 
server  loop  and  the  LQR  loop  [62]  [63]. 


Figure  6.4-  LQG/LTR  Controller  Experimental  Run  -  Traj  2 


6.3.1  SCB  Background 

Given  a  system  of  the  form 

x(t)  =Ax(t)+Bu(t)  (6.19) 

y(t)  =  Cx(t)  (6.20) 

SCB  transforms  this  system  using  3  transformations.  Tj  transforms  the  state,  r2 

transforms  the  output,  and  r3  transforms  the  control  of  the  original  system  to  SCB 

form.  Equations  show  the  transformations  to  SCB  form. 


(6.21) 


(6.22) 
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where 


(6.23) 


-  Xg  are  the  states  not  coupled  directly  to  the  input  or  output 

-  X|,  are  the  states  not  directly  affected  by  the  input  but  are  output  as 

-  Xc  are  the  states  affected  by  the  input  v  but  are  not  directly  output 

-  Xf  are  the  states  directly  affected  by  the  input  u  and  affect  output  yf 

-  yf  are  the  fast  outputs 

-  y^  are  the  slow  outputs 

-  u  and  V  are  the  SCB  inputs 

According  to  SCB,  the  size  of  the  x^,  Xj,,  x^.,  Xf  vectors  determine  several  impor¬ 
tant  system  characteristics.  Namely,  the  size  of  x^  determines  the  number  of  system 
invariant  zeros.  Further,  x^  can  be  divided  into  3  subvectors,  x^.,  x^q,  and  x^^.  The 
number  of  invariant  zeros  in  the  left-half  plane  correspond  to  the  dimension  of  x^.. 
Similarly,  the  number  of  invariant  zeros  on  the  imaginary  axis  is  equal  to  the  dimen¬ 
sion  of  x^o-  Finally,  the  number  of  non-minimum-phase  zeros  is  equal  to  the 
dimension  of  Xa+. 

Right-invertibility  of  a  system  is  determined  by  the  dimension  of  Xf,.  If  xj,  has  a 
dimension  of  0,  then  the  system  is  right-invertible.  Similarly,  left-invertability  is  de¬ 
termined  in  the  same  manner  by  the  dimension  of  Lastly,  the  number  of  infinite 
zeros  is  equal  to  the  number  of  fast  states,  Xf. 

Another  important  feature  of  SCB  decomposition  is  that  the  invariant  zeros  can 
be  calculated.  The  SCB  A  matrix  has  a  sub-matrix  associated  with  the  states  of 
Xjj.  As  it  turns  out,  the  eigenvalues  of  the  A^a  submatrix  are  the  invariant  zeros  of  the 
original  system. 


6.3.2  SCB  Plant  Analysis 

An  SCB  analysis  was  applied  to  the  original  40th  order  arm  model  for  analysis 
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purposes.  When  performing  an  open-loop  analysis,  the  MATLAB  tzero.m  command 
stated  that  there  were  no  invariant  zeros.  Obviously,  this  was  an  error.  Using  the  SCB 
toolbox  [41],  the  40th  order  system  was  discovered  to  have  36  minimum-phase  in¬ 
variant  zeros  and  4  infinite  zeros. 

As  stated  earlier,  all  the  controllers  in  this  paper  used  the  18th  order  model,  so  an 
analysis  was  performed  on  this  model.  The  reduced  model  possessed  14  minimum- 
phase  zeros,  2  large  non-minimum-phase  zeros,  and  2  infinite  zeros.  Table  6.4  lists 
the  18th  order  invariant  zeros.  The  non-minimum  phase  zeros  are  so  large  that  they 
are  essentially  infinite  zeros  and  shouldn’t  limit  the  bandwidth  of  the  designed 
controllers. 


Table  6.4  -  18th  Order  Model  Invariant  Zeros 


Invariant  Zeros 

1.8641e+U5+  1.5/y/e+05i 

1.8641e+05-  1.5797e+05i 

-3.6493e+02+  1.5033e+03i 

-3.6493e+02-  1.5033e+03i 

-5.3162e+01+  5.6874e+02i 

-5.3162e+01-  5.6874e+02i 

-7.3953e+01+  4.9774e+02i 

-7.3953e+01-  4.9774e+02i 

-4.9655e+00+  1.7204e+02i 

-4.9655e+00-  1.7204e+02i 

-1.3772e+00+  8.1386e+01i 

-1.3772e+00-  8.1386e+01i 

-6.8069e-02+  1.7224e+01 

-6.8069e-02-  1.7224e+01i 

-8.7010e-03+  5.4323e+00i 

-8.7010e-03-  5.4323e+00i 

6.3.3  SCB/LTR  Background 

Loop  Transfer  Recovery  based  on  SCB  form  has  the  advantage  that  it  directly 
minimizes  the  error  between  the  loop  transfer  function  of  the  observer  and  the  target 
loop  of  the  LQR  design  (and  its  desirable  robustness).  Figure  6.5  shows  a  block  dia¬ 
gram  of  the  target  LQR  loop. 
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The  corresponding  state  feedback  target  loop  is  given  in  Equation  (6.24). 

L^is)  =  F{sI-A)~^B  (6.24) 

In  contrast.  Figure  6.6  shows  an  output  feedback  loop.  Here,  y  is  the  output  given 
by  Equation  (6.25). 

y{t)  =  Cx(t)  +Du(t)  (6.25) 

The  corresponding  loop  equation  is  given  in  Equation  (6.26). 

L  (s)  =  C  (s)  P  (s)  (6.26) 


Plant : 

i:=Ax+Bu 

Uf=Fx 


Figure  6.5-  LQR  Target  Loop 


Figure  6.6-  Output  Feedback  Loop 

Loop  Transfer  Recovery  is  guaranteed  if  the  system  between  the  sensors  and  the 
control  input  is  minimum-phase  and  left-invertible.  Also,  if  there  are  no  infinite  ze¬ 
ros,  then  exact  recovery  is  possible  with  finite  gains.  Otherwise,  asymptotic  recovery 
is  possible  (like  LQG/LTR).  The  LTR  problem  is  to  minimize  the  error  between  Lj(s) 
and  L(s),  or 


E(s)  =Lis)-L^is) 


(6.27) 


The  error  can  also  be  represented  in  the  form  of  Equation  (6.28)  [62]. 

E(s)  =  M/s)  [I^+M/s)]'^[i^  +  F(sI-A)-^b]  (6.28) 

where  Mf(s)  is  the  recovery  matrix,  and  F,  A,  and  B  are  determined  from  the  LQR 

target  loop.  The  Mf(s)  matrix  can  be  further  broken  down,  as  shown  in  Equation 
(6.29). 

=  FM^(s)  =  (sI-A+K^)~\B-Ji::jD)  (6.29) 

Thus,  the  LTR  problem  reduces  down  to  minimizing  the  2-norm  of  the  Mf{s)  ma¬ 
trix.  The  only  variable  to  optimize  is  the  Kf  observer  gain  matrix. 

There  are  two  methods  to  minimize  the  Mf(s)  matrix  -  the  Riccati  approach  and 

the  Eigen  Assignment  approach,  both  of  which  are  detailed  in  [63].  For  this  research, 
only  the  Riccati  approach  was  used,  so  a  brief  description  of  this  approach  is  given 

next.  For  the  Riccati  approach,  the  transpose  of  Mf(s)  is  used  to  represent  a  ficti¬ 
tious  plant  to  find  the  optimal  Kf.  The  transpose  has  the  following  form: 

~T  (  T  T  rV  T  T  tV^ 

Mj  =  -D  Kf  j^sI-A  +C  j  (6.30) 

The  fictitious  plant  has  the  form  shown  in  Equation  (6.31)  and  Equation  (6.32). 

(6.31) 

yficC')  (6.32) 

With  this  fictitious  plant,  the  cost  function  for  the  2-norm  of  Mf{s)  reduces  down  to 
Equation  (6.33). 

=  \{ycrilycrit  +  ^^^^dt  (6.33) 

0 

where  £  is  a  very  small  value.  Having  8  approach  zero  is  equivalent  to  having  the 
LQG/LTR  process  noise  go  to  infinity  in  the  sense  that  asymptotic  recovery  is 
achieved  for  small  values  of  8  and  large  values  of  process  noise.  The  plant  is  mini¬ 
mized  like  a  normal  LQR  problem,  where  we  substitute  the  quadruple  (A,B,C,D)  by 
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The  resulting  LTR  controller  is  formed  in  the  same  manner  as  the  LQG  control¬ 
ler.  The  only  difference  is  that  the  LQE  optimal  estimator  gain  K  is  replaced  by  the 
LTR  optimal  estimator  gain  Kf. 

6.3.4  SCB/LTR  Application 

As  with  the  LQG  controller,  only  the  non-integral  states  need  estimation.  After 
the  Kf  gain  is  determined,  the  integral  states  are  reattached  as  before  to  find  the  final 
controller.  The  SCB/LTR  controller,  being  a  full-order  observer  design,  has  the  same 
order  as  the  plant.  In  this  case,  the  LTR  controller  is  20th  order.  We  know  from  the 
18th  order  plant  SCB  analysis  that  there  are  2  non-minimum-phase  zeros,  so  com¬ 
plete  recovery  is  not  possible.  Also,  there  are  2  infinite  zeros  so  at  best  only 
asymptotic  recovery  can  be  achieved  with  high  gains. 

For  this  design,  the  full-state  feedback  gain  F  is  simply  Ge,  the  LQR  gain  for  the 
estimated  states.  Also,  since  the  design  must  be  implemented  on  hardware,  the  gains 
must  be  limited.  For  an  SCB/LTR  design,  this  is  possible  by  increasing  the  value  of 
£  placing  less  emphasis  on  the  minimization  of  the  recovery  error.  The  value  used  for 
this  design  is  £=.01.  Table  6.2  shows  the  estimator  Kf  gains  achieved  for  this 
application. 


Table  6.5  -  LTR  Optimal  Kf 


Estimated 

State 

yi 

y2 

4.Ur/6e-Ul 

-5.0529e+00 

9.2367e-01 

-6.7796e+00 

^3 

-8.6840e+00 

8.6076e+01 

X4 

-3.2182e+01 

1.0947e+02 

X5 

6.8895e+01 

9.2625e+01 

X6 

1.2994e+01 

4.0426e+01 

x? 

-1.8468e-01 

-2.2199e+01 

Xg 

2.4941e+00 

7.554  le+01 

X9 

6.0090e+00 

2.7271e+01 

XlO 

-7.6128e+00 

5.6034e+01 

Xll 

-5.6783e+01 

3.5197e+0 

Xl2 

-7.7307e+00 

1.1920e+00 
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Table  6.5  -  LTR  Optimal  Kf 


Estimated 

State 

yi 

y2 

xi3 

4.6U55e-0I 

6.3234e-Ul 

Xi4 

I.9505e-01 

3.1258e-i-00 

Xi5 

8.4164e-02 

3.0838e-01 

Xl6 

-8.1887e-02 

-5.1376e-02 

Xl7 

-5.3406e-0I 

-3.8686e-0I 

Xi8 

1.6184e-01 

3.3696e-0I 

The  controller  was  implemented  at  a  1  ms  sampling  time,  like  the  LQG  and  LQG/ 
LTR  controllers.  Simulation  results  were  again  identical  to  LQG/LTR  since  the  sim¬ 
ulation  assumes  perfect  initial  knowledge  of  the  states.  Figure  6.7  shows  the 
experimental  evaluation  of  the  SCB/LTR  controller  on  the  arm  along  trajectory  2. 
Once  again,  the  response  is  similar  to  the  LQG  and  LQG/LTR  controllers. 


Figure  6.7-  LTR  Controiier  Experimental  Run  -  Traj  2 


6.4  Controller  Reduction 

In  cases  where  the  plant  order  is  extremely  large,  often  times  controiier  reduction 
is  required  for  implementation.  There  are  two  primary  methods  for  reducing  the  order 
of  controllers,  and  both  are  applied  to  the  LQG/LTR  and  LTR  controllers.  It  was  ex- 
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pected  that  both  controllers  would  reduce  to  about  the  same  order  since  both  used  the 
same  LQR  state  feedback  gains,  which  primarily  determine  the  dynamic  perfor¬ 
mance  of  the  system.  The  first  method  is  balanced  truncation  [52]  and  the  second  is 
modal  residualization  [26]. 

When  reducing  the  controllers,  it  is  important  to  retain  the  2  integral  controller 
states.  Thus,  these  modes  are  removed,  like  in  the  observer  design,  and  added  back 
to  the  system  after  reduction  of  the  remaining  controller  states. 


Figure  6.8-  LQG/LTR  7th  Order  Reduced  Controller  Exp  Response-  Traj  2 


With  balanced  truncation,  the  system  is  balanced  and  arranged  from  the  largest 
Hankel  singular  value  (HSV)  to  the  smallest.  Since  the  HSVs  represent  the  control¬ 
lability  and  observability  of  the  system,  those  modes  with  small  HSVs  don’t 
contribute  much  to  the  dynamic  performance  of  the  system.  Thus,  they  can  be  re¬ 
moved  and  reduce  the  system  order  with  minimal  loss  of  performance.  When 
balanced  truncation  was  applied  to  the  LQG/LTR  and  the  LTR  controllers,  both  were 
only  stable  for  19th  order  reduced  controllers.  The  reason  for  this  is  that  the  HSVs 
were  all  important  (the  smallest  HSV  was  about  2  in  both  cases).  Because  these  con¬ 
trollers  had  degraded  performance  in  simulation,  they  were  not  implemented 
experimentally. 
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Modal  residualization  puts  the  system  in  modal  form,  with  all  modes  decoupled 
in  the  controller  system  matrix.  The  fastest  modes  are  those  with  the  largest  real 
eigenvalues,  and  these  modes  don’t  have  as  much  of  an  effect  on  the  performance  be¬ 
cause  they  happen  so  quickly.  The  slow  modes  tend  to  dominate  system  performance. 
Application  of  modal  residualization  to  both  controllers  produced  7th  order  control¬ 
lers  with  only  a  small  degradation  in  performance. 


7th  Ord  LQG-UTR,  Exp  vs.  Sim,  Tra|  2 

Figure  6.9-  LQG/LTR  7th  Order  Reduced  Controller  Exp  vs.  Sim,  9i  -  Traj  2 


7th  Ord  LQG-LTR,  Exp  vs.  Sim,  TraJ  2 


Figure  6.10-  LQG/LTR  7th  Order  Reduced  Controller  Exp  vs.  Sim,  02-  Traj  2 
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The  LQG/LTR  7th  order  controller  was  implemented  on  the  arm  by  applying  a 
Tustin  transformation  for  a  1  ms  sampling  time.  Figure  6.8  shows  the  experimental 
response  of  the  7th  order  LQG/LTR  controller,  while  Figure  6.9  and  Figure  6.10 
show  the  experimental  results  as  compared  to  simulation  for  the  same  controller. 
Note  that  the  performance  is  comparable  to  the  full-order  LQG/LTR  controller  and 
that  the  simulation  catches  the  correct  trends  in  both  links. 

The  SCB/LTR  reduced  7th  order  controller  was  also  implemented  and  tested  on 
the  arm  at  1  ms  sampling  time.  Figure  6. 1 1  displays  the  experimental  response  on  the 
arm  for  trajectory  2,  which  is  similar  to  the  LQG/LTR  response.  Although  not  shown, 
the  simulation  results  are  quite  similar  to  the  LQG/LTR  simulation  results. 


01.2  3  4  5  6  7  8  9  10 

t 

7th  Ord  LTR  Exp,  TraJ  2 

Figure  6.11-  SCB/LTR  7th  Order  Reduced  Controller  Exp  Response-  Traj  2 
6.5  Control  Bandwidths 

Since  all  the  controllers  are  based  on  the  same  LQR  gains,  and  the  performance 
of  each  are  similar,  they  should  have  similar  control  bandwidths.  It  was  noticed  that 
the  LQG-class  controllers  never  experienced  high-frequency  vibrations,  as  seen  from 
the  evaluation  of  the  control  bandwidths.  Figure  6.12  shows  the  6i/6icmd  control 
bandwidth  plot,  while  Figure  6.9  shows  the  62/62ctnd  control  bandwidth  plot.  The 


bandwidth  for  the  first  loop  is  about  2  rad/s,  while  the  second  loop  also  has  a  band 
width  of  about  2  rad/s. 


Figure  6.12-  0i/0icmd  LQG  Control  Bandwidth 


K 

I 

Q. 


LQG-LTR  BW  ,  Th_2-Th_2cmd 


Figure  6.13-  02/02cmd  LQG  Control  Bandwidth 
6.6  Observations 

Clearly,  the  reduced-order  controllers  provide  comparable  performance  to  the 
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full-order  controllers.  Thus,  implementation  of  LQG-class  controllers  may  allow  the 
use  of  reduced-order  controllers.  It  is  interesting  to  note  that  the  20th  order  control¬ 
lers  ran  at  a  0.4  ms  sampling  time,  while  the  7th  order  controllers  ran  at  a  0.3  ms 
sampling  time.  In  comparison,  4th  order  controllers  and  PID  controllers  ran  at  0.2  ms 
sampling  times.  The  fastest  sampling  time  allowed  by  the  data  acquisition  system  is 
0.1  ms.  Thus,  by  reducing  the  controller  order  enough,  you  can  sample  at  twice  the 
rate.  This  might  be  critical  in  some  applications. 

Appendix  C  shows  additional  plots  for  all  of  the  LQG-class  controllers  for  differ¬ 
ent  trajectories.  In  addition,  comparison  plots  show  how  each  controller  performs 
along  the  different  trajectories.  Performance  of  the  controllers  remain  about  the  same 
regardless  of  the  trajectory. 


7  SANDY  Background 


Many  MIMO  designs  make  use  of  optimal  control  through  linear-quadratic-reg¬ 
ulator  (LQR)  and  linear-quadratic-gaussian  (LQG)  techniques.  Both  LQR  and  LQG 
are  based  on  solutions  to  the  appropriate  Riccati  equations,  and  are  well-documented 
[1]  [35].  LQR  has  the  advantage  of  guaranteed  robustness  but  the  disadvantage  of  re¬ 
quiring  full-state  feedback.  LQG  has  the  advantage  that  it  can  be  implemented  based 
on  available  sensors,  and  the  disadvantage  of  a  loss  in  robustness  and  the  use  of  high 
controller  order  (the  same  order  as  the  plant).  To  remedy  the  loss  of  robustness,  LQG/ 
LTR  Loop  Transfer  Recovery  (LTR)  has  been  developed  [20].  However  LQG/LTR 
still  suffers  the  disadvantage  of  a  high-order  controller. 

To  combat  the  high  controller  order,  techniques  such  as  balanced  truncation  or 
modal  residualization  are  applied  to  the  controller  in  hopes  that  a  satisfactory  reduced 
order  controller  can  be  obtained.  Unfortunately,  there  is  no  guarantee  that  a  suitable 
low-order  controller  can  be  found  through  model  reduction.  A  systematic  approach 
to  the  design  of  low-order  controllers  has  been  developed  by  Ly  and  his  coworkers 
[44]  [45]  [46].  This  approach  is  based  on  direct  optimization  and  is  implemented  in 
the  computer  code  SANDY  [45].  SANDY  allows  specification  of  additional  design 
constraints  and  of  the  desired  controller  structure. 

In  order  to  use  SANDY,  plant  models  must  be  represented  in  the  format  described 
in  equations  (7.1)-(7.3).  Figure  7.1  gives  a  block  diagram  view  of  this  plant  descrip¬ 
tion,  which  has  a  typical  feedback  control  system  structure. 

The  plant  models  are  described  by 


X  (t)  =  fx  (t)  +  G'u  (t)  +  r  w'  (t) 

(7.1) 

>.■(»)  =  h‘/ W>- dI/ (t)  (1) 

(7.2) 

y' (0  =  +dL“'(0 

(7.3) 

where 

-  x’(t)  is  a  state  vector  of  dimension  n. 

-  u’(t)  is  a  control  vector  of  dimension  of  m. 
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-  w*(t)  is  a  disturbance/command  input  vector  of  dimension  m’. 

-  y^it)  is  a  measurement  vector  of  dimension  p. 

-  (t)  is  a  vector  of  dimension  p’  containing  the  design  criterion  variables. 
The  superscript  i  corresponds  to  the  ith  plant  in  a  multiple  plant  setting. 


Figure  7.1-  Sandy  Feedback  Control  Structure 

The  SANDY  program  optimizes  the  following  design  objective  function  given  by 

N 

JOf)  =  S  Vilw 

i  =  1 

where  the  term  (i=l,Npn,)  is  a  quadratic  performance  index  evaluated  at  the  ith 
plant,  and  Npj^  is  the  number  of  design  plant  models.  Also,  each  plant  may  have  a 

weighting  denoted  by  Wpj.  Notice  that  each  term  has  a  variable  final  time  to  allow 

evaluation  of  the  design  objective  for  systems  which  are  initially  unstable. 

For  the  deterministic  case  (initial  condition  response),  the  cost  function  is: 

r  ^  2a. 

=  (1/2) “‘'[yf  (OGV' (0  +u^it)R'uit)]dt  (7.5) 
0 

For  random  impulsive  disturbances,  the  objective  function  is: 
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=  (1/2)  (0(2j1(0  +u^{t)R'u\t)']dt  (7.6) 

0 

where  the  expectation  operator  E[]  is  used  to  handle  random  inputs.  Finally,  for  ran¬ 
dom  white-noise  disturbances,  the  following  equation  is  used: 


(7.7) 

In  all  the  above  cost  functions,  the  Q‘  terms  are  weighting  on  the  criterion  outputs  for 


a.f 

each  plant,  and  R  is  the  weighting  on  the  controls.  The  e  '  term  is  a  destabilization 
term  for  each  plant. 

In  addition,  constraints  can  be  applied  to  the  optimization.  Some  of  the  con¬ 
straints  available  in  SANDY  are  direct  bounds  on  the  design  parameters  in  the 
SANDY  controller  matrices,  nonlinear  constraints  on  the  covariance  responses  of  the 
outputs  and  controls,  and  nonlinear  constraints  on  the  closed-loop  stability  by  speci¬ 
fying  the  maximum  real  part  of  the  closed-loop  eigenvalues  and  the  minimum 
damping  of  the  complex  roots.  Additionally,  the  command  inputs  can  be  filtered  to 
match  the  desired  input  responses. 

Once  all  the  constraints  and  the  weightings  have  been  specified,  the  designer  then 
chooses  the  structure  of  the  controller  by  specifying  an  initial  guess  to  the  controller 
A,B,C,D  matrices.  Next,  the  variables  to  be  optimized  are  specified.  SANDY  then 
minimizes  the  cost  function  of  equation  (7.4)  with  respect  to  the  design  parameters 
in  the  A,B,C,D  matrices  of  the  controller  and  comes  up  with  a  single  controller  for 
all  the  design  plant  conditions. 


8  SANDY  Double- Angle  Control  Design 


The  simplest  design  is  to  optimize  SANDY  to  the  error  between  the  reference 
commands  and  the  actual  joint  angles.  Thus,  the  optimization  is  based  on  the  two 
measured  angle  sensors  and  the  two  angle  reference  trajectories. 


8.1  SANDY  Design 

For  this  design  problem,  one  plant  condition  was  developed  with  a  filtered  step 
command  input  for  the  first  joint  angle,  and  a  second  plant  with  a  filtered  step  com¬ 


mand  input  for  the  second  joint  angle.  The  criterion  variables  are  Gj ,  02,  error_0i, 
and  error_02,  respectively.  There  are  two  controls,  torque_l  and  torque_2  and  two 
sensors,  0}  and  02-  The  criterion  variables  are  used  only  in  the  cost  function,  and  can 
contain  variables  other  than  those  available  as  sensors.  Thus,  the  SANDY  controller 
should  have  two  inputs  (the  sensors)  and  two  outputs  (the  torques).  Values  of  the  non¬ 
linear  constraints  were  a  direct  constraint  on  the  control  covariance  of  torque  1  for 
plant  1  of  10  Nm,  a  constraint  on  the  maximum  real  part  for  the  closed-loop  eigen¬ 
value  of  -.25,  no  weighting  on  the  control  penalties  Rj  and  R2,  and 
Qj=diagonal(0,0,l,0)  with  Q2  =  diagonal(0,0,0,l).  The  initial  guess  of  the  SANDY 
controller,  based  on  the  PID  controllers  in  the  earlier  chapters,  is: 


0  0  0  0 

0  0  0  0 

0  0-8  0 
0  0  0  -10 


,C  = 


0.1  0  8  0 
0  0.11  0  10 


,D  = 


This  is  a  fourth  order  controller  with  2  integral  states  and  a  first  order  filter  for  each 
of  the  0j  and  02  sensor  inputs.  The  integral  states  are  added  to  reduce  the  steady  state 


error  in  the  tracking  of  the  joint  angles.  The  controller  design  variables  are  A(3,3), 
A(4,4),  B(3,l),  B(4,2),  C(l,3),  and  C(2,4)  for  the  first  order  filters.  In  addition, 
C(1,1),C(1,2),C(2,1),  and  C(2,2)  are  the  design  variables  for  the  integral  gains.  Final¬ 
ly,  the  entire  D  matrix  is  optimized  for  the  proportional  gains. 

After  300  function  calls,  the  design  optimization  produced  the  following  SANDY 
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controller 


0  0 

0 

0 

1 

0 

0  0 

0 

0 

= 

0 

1 

0  0 

-3.91 

0 

-7.80 

0 

0  0 

0 

-7.37_ 

0 

-14.93_ 

C  = 


0.172  -3.31  11.08  2.97 
p.024  3.60  -2.91  11. 0_ 


D  = 


25.01  -1.20 
-111  29.11 


(8.2) 


The  cost  function  value  associated  with  plant  1  is  0.1345  and  the  cost  for  plant  2  is 
0.0311. 


The  controller  is  then  discretized  using  a  Tustin  transformation  at  a  1  ms  sam¬ 
pling  time.  The  difference  between  a  1  ms  simulation  and  a  continuous  simulation 
was  negligible  for  this  controller,  verifying  the  initial  assumption  that  a  continuous 
SANDY  design  should  provide  a  good  digital  controller  at  this  low  sample  time  (a 
1000  Hz  sampling  rate).  If  digital  considerations  are  desired,  more  stringent  con¬ 
straints  may  be  placed  on  the  control  covariances  to  keep  the  gain  on  the  controller 
low  making  it  more  applicable  for  higher  sampling  times  (and  thus  lower  sampling 
rates). 


01  23456789  10 

t 

SANDY  TraJ  2 


Figure  8.1-  SANDY  Experimental  Run,  Trajectory  2 
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SANDY  Exp  ve  Sim,  TraJ  2 

Figure  8.2-  SANDY  Experimental  vs.  Simulation,  Trajectory  2,  Gj 

Figure  8.1  shows  the  experimental  run  along  trajectory  2.  Figure  8.2  and  Figure  8.3 
show  the  experimental  results  vs.  simulation  for  the  SANDY  controller.  Notice  that 
the  simulation  captures  much  of  the  actual  dynamics  of  the  experiment.  The  SANDY 
controller  responds  quickly  with  minimal  overshoot  and  small  steady-state  error. 


SANDY  Exp  vs  Sim,  Traj  2 


Figure  8.3-  SANDY  Experimental  vs.  Simulation,  Trajectory  2, 02 


8.2  SANDY  Control  Bandwidth 


It  is  important  to  realize  the  capability  of  the  controller  when  evaluating  the  ex¬ 
perimental  performance.  For  the  TLA,  the  control  bandwidth  is  defined  from  the 
Bode  plot  of  0i/0icmd  for  the  first  motor  and  02/02cmd  for  the  second  motor.  Again, 
the  convention  used  in  this  research  is  that  the  bandwidth  is  where  the  magnitude  first 
crosses  minus  3dB  magnitude.  If  the  control  task  is  greater  than  the  bandwidth,  then 
the  controller  won’t  be  able  to  achieve  the  desired  performance.  The  primary  control 
bandwidth  for  the  first  motor  (shoulder)  is  about  3.5  rad/sec  as  shown  in  Figure  8.4, 
while  the  primary  control  bandwidth  for  the  second  motor  (elbow)  is  around  1 1  rad/ 
sec  as  shown  in  Figure  8.5.  Notice  that  in  both  cases,  there  are  multiple  0  dB  cross¬ 
ings  due  to  high  frequency  resonances  associated  with  the  flexible  modes. 


.01  .1  1  10 

Frequency  [rad/sec] 

Th1-Th1_cmd  Bandwidth,  SANDY 


Figure  8.4-  Control  Bandwidth  for  the  Shoulder  Motor 

Notice  that  both  bmdwidth  plots  have  peaks  above  0  dB,  meaning  those  frequen¬ 
cies  would  be  amplified.  However,  in  actuality  the  commands  are  filtered  through  a 


Frequency  [rad/sec] 
Th2-Th2_cmd  Bandwidth,  SANDY 


SANDY  Original  4th  Order  (pref=1),  th1-th1_cmd  BW 


Figure  8.6-  SANDY  Bandwidth  With  Prefilter,  Shoulder  Motor 
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spline,  and  this  must  be  accounted  for  in  the  bandwidth  plots.  Also,  the  SANDY  op¬ 
timization  assumes  a  1st  order  command  prefilter  at  1  rad/s  is  present.  Thus,  band¬ 
width  plots  with  the  command  prefilter  are  included.  Figure  8.6  shows  the  filtered 

r 

bandwidth  for  the  shoulder  motor  is  about  1  rad/s  while  Figure  8.7shows  that  the  el¬ 
bow  motor  also  has  a  control  bandwidth  of  1  rad/s.  Notice  that  with  the  command 
prefilter  at  1  rad/s  the  magnitude  never  exceeds  0  dB,  which  is  desirable  for  a  control 
bandwidth  response. 


.1  1  10  100 
Frequency  [rad/sec] 

SANDY  original  4th  Order  (pref=1),  th2-th2_cmd  BW 

Figure  8.7-  SANDY  Bandwidth  With  Prefilter,  Elbow  Motor 

8.3  Experimental  Comparison  of  SANDY  and  PID  Controllers 

The  purpose  of  this  section  is  to  compare  the  experimental  results  of  the  SANDY 
and  PID  controllers.  Although  in  this  case  the  SANDY  controller  displays  better 
overall  response  in  terms  of  tracking  error,  it  should  be  pointed  out  that  more  design 
time  was  spent  on  the  SANDY  controller  than  on  the  PID  controller.  Since  both  are 
effectively  4th  order  controllers,  they  should  have  similar  performance  if  an  equal 
amount  of  effort  is  spent  on  each  of  the  designs.  The  one  advantage  the  SANDY  con¬ 
troller  has  over  a  straight  PID,  however,  is  the  ability  to  provide  crossfeed  informa- 
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tion  from  one  angle  into  the  control  for  the  other.  This  is  up  to  the  designer  in  the 
choice  of  the  SANDY  structure. 

The  baseline  comparison  is  for  all  the  three  trajectories  listed  in  Chapter  5.  Tra¬ 
jectory  2  comparisons  are  found  in  Figure  8.8  and  Figure  8.9. 


SANDY  vs  PID,  Traj  2 

Figure  8.8-  PID  vs.  SANDY,  Traj  2, 


Figure  8.9-  PID  vs.  SANDY,  Traj  2,  02 
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The  SANDY  controller  responds  more  rapidly  and  with  less  overshoot  than  the  PID 
controller  for  link  1  and  has  comparable  overshoot  but  with  lower  settling  time  in  link 
2. 

To  test  the  robustness  of  each  respective  controller,  a  different  tip  attachment  is 
used.  The  new  tip,  bracket,  and  bowl  weigh  725  grams  total  compared  to  351  grams 
for  the  original  tip. 


t 

PID  vs  SANDY  >  Robust,  Tra]  2 


Figure  8.10-  Robust  PID  vs.  SANDY,  Traj  1,  Oj 


PID  vs  SANDY  -  Robust,  Tra|  2 


Figure  8.11-  Robust  PID  vs.  SANDY,  Traj  1, 62 
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In  addition,  to  provide  variable  dynamics,  a  heavy  metal  ball  with  a  weight  of 
1.043  kg  is  placed  in  the  middle  of  the  cup  and  is  free  to  move  throughout  the  slew. 
Trajectory  2  results  are  displayed  in  Figure  8.10  and  Figure  8.11.  This  time,  the 
SANDY  controller  response  was  much  faster  and  had  significantly  less  overshoot. 

Appendix  B  contains  additional  plots  of  the  SANDY  controller  along  different 
trajectories.  In  addition,  comparison  plots  with  the  PID  controller  are  included  along 
the  different  trajectories.  The  trends  noted  for  trajectory  2  in  this  chapter  are  the  same 
for  all  the  tested  trajectories  in  Appendix  B. 


9  Sandy  Robust  Design  and  Endpoint  Position  Test 

As  an  alternative  to  controlling  the  hub  endpoint  position  through  the  hub  angles, 
the  optimization  can  be  reformulated  using  forward  and  inverse  kinematics  in  the 
model  to  control  the  x  and  y  endpoint  positions. 

9.1  Forward  Kinematics 

The  endpoint  position  can  be  predicted  from  the  hub  angles  through  the  forward 
kinematics.  It  is  important  to  realize  that  this  technique  is  very  good  for  rigid  manip¬ 
ulators,  but  provides  only  an  estimate  of  the  endpoint  position  for  flexible 
manipulators.  Figure  9.1  shows  the  conventions  for  forward  kinematics. 


Figure  9.1-  Forward  Kinematic  Conventions 


The  final  endpoint  position  is  actually  a  contribution  from  the  link  1  and  link  2 
positions.  The  length  of  the  links,  d^  and  d2,  is  related  to  parameters  listed  in  Appen¬ 
dix  A. 


'  (9-1) 

If  the  desired  endpoint  is  at  the  end  of  the  end-effector,  then  0(2  can  be  increased  to 
move  the  endpoint  to  the  desired  location.  For  the  arm  used  in,  this  research, 
di=0.6363  m  and  d2  =  0.6096  m. 

The  joint  angle  Oj  is  associated  with  the  first  link  and  the  angle  02  corresponds  to 
the  second  link.  To  obtain  the  endpoint  position,  a  new  angle  a  is  defined  in  Equation 
(9.2). 
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a  =  9O-0j-e2  (9.2) 

Using  these  equations  and  definitions  results  in  the  following  equations  for  x^p 

and  Yep. 

Xep  =  JjCos(0j)  +J2sin(a)  (9.3) 

y^p  =  Jjsin  (0j)  +  J2C0S  (a)  (9.4) 

The  sign  convention  for  this  scheme  is  CCW  =  +  for  the  angles  aAd  +  is  based  on  the 
right-hand  rule  for  the  cartesian  directions,  as  shown  in  Figure  9.1. 

9.2  Inverse  Kinematics 

The  inverse  kinematics  take  the  known  x^p  and  ygp  locations,  and  calculate  the  0] 
and  02  angles.  This  is  trickier  than  the  forward  kinematic  case  because  there  are  an 
infinite  number  of  mathematical  solutions  for  0j  and  02  given  a  specific  Xgp  and  ygp. 
Fortunately,  there  are  only  2  physical  redundancies  for  the  angle  positions,  as  shown 
in  Figure  9.2.  The  rest  are  just  mathematical  equivalents  for  the  2  physical  angles,  dif¬ 
fering  by  adding  multiples  of  360°. 


Physical  Configuration  1 
Physical  Configuration  2 


Qij :  i=Config  # 
j  =  Hub  Angle 


+ 


ccw=+ 

yep 


Figure  9.2-  TLA  Physical  Angle  Redundancy 


For  the  TLA  manipulator  used  in  this  research,  only  physical  configuration  1  is 
possible.  The  second  motor  has  stops  which  prevent  02  from  ever  reaching  negative 
values,  so  our  physical  system  has  only  one  possible  inverse  kinematic  solution. 
Making  use  of  the  physical  configuration  makes  a  unique  analytic  solution  possible 
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by  ensuring  the  02  angle  is  always  positive.  The  inverse  kinematics  can  be  solved  us¬ 
ing  the  law  of  cosines  for  the  triangle  shown  in  Figure  9.3. 


Figure  9.3-  Inverse  Kinematic  Angle  Conventions 
The  general  form  of  the  law  of  cosines  is 

-2bc cos  (A)  (9.5) 

Table  9. 1  lists  the  correspondence  between  the  generic  terms  in  the  law  of  cosines  and 

the  TLA  application. 


Table  9.1  -  Law  of  Cosine  TLA  Terms 


Generic 

Term 

TLA  Term 

Units 

a 

^2 

m 

b 

dl 

m 

c 

r 

m 

A 

0-01 

rad 

B 

N/A 

rad 

C 

180-02 

rad 

where  r  and  6  are  the  polar  coordinates  for  the  endpoint.  Specifically, 
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9.3  TLA  Application 

The  optical  encoder  mounted  to  the  second  motor  shaft  is  oriented  so  that  its  in¬ 
dex  or  zero  reading  point  is  actually  at  90°.  Thus,  to  achieve  the  correct  kinematics, 
90°  must  be  added  to  the  angle  read  from  the  encoder  for  02.  The  resulting  kinematic 
relationship  is  shown  in  Table  9.2. 


Table  9.2  -  Some  TLA  Kinematic  Relationships 


01  o 

02  0 

02+90  0 

Xep  (m) 

Yep  (m) 

0 

0 

90 

.6363 

.6096 

0 

-90 

0 

1.246 

0 

0 

-20 

70 

.8448 

.5728 

0 

+20 

110 

.4278 

.5728 

-20 

0 

90 

.8064 

.3552 

-20 

-20 

70 

.9898 

.2494 

+20 

0 

90 

1 

.3894 

.7905 
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Table  9.2  >  Some  TLA  Kinematic  Relationships 


01  O 

02  n 

02+90  0 

Xgp  (m) 

Yep  (m) 

+20 

+20 

110 

.2061 

.6846 

-10 

-10 

80 

.8351 

.4623 

+10 

+10 

100 

.4181 

.6833 

-40 

-40 

50 

1.09 

-.300 

It  is  interesting  to  note  that  when  dealing  with  the  endpoint  position,  the  actuators 
are  still  located  at  the  hubs.  In  other  words,  the  endpoint  position  must  be  converted 
to  hub  angles  through  inverse  kinematics  to  produce  angle  reference  signals.  The  ac¬ 
tual  angles  read  from  the  encoders  are  compared  to  the  reference  signals  to  produce 
error  signals,  which  are  fed  into  the  joint  controller.  Figure  9.4  shows  the  block  dia¬ 
gram  implementation  for  such  a  control  scheme. 


30-SEP-94 


Figure  9.4-  TLA  Endpoint  Trajectory  Implementation 
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9.4  Endpoint  Trajectory  Generation 

In  order  to  test  endpoint  tracking,  several  trajectories  are  used.  These  trajectories 
provide  differing  dynamic  situations  for  the  TLA,  and  provide  a  test  of  robustness  to 
trajectories. 

9.4.1  Straight  Line 

One  of  the  simplest  yet  most  common  trajectories  in  robotics  is  a  straight  line. 
Going  between  any  2  points  in  two-dimensional  cartesian  space  constitutes  a  straight 
line  [56].  In  order  to  smooth  the  commands,  a  quintic  spline  in  x  and  y  is  commanded 
between  the  start  and  end  points.  This  is  similar  to  the  quintic  spline  used  to  com¬ 
mand  hub  angles.  The  resulting  reference  trajectory  is  shown  in  Figure  9.5. 


Lino  Rof  Tra|.  .21  m 

Figure  9.5-  0.21m  Straight  Line  Reference  Trajectory 

It  is  interesting  to  compare  the  hub  angles  and  endpoint  positions  for  a  spline  in¬ 
put  of  the  angles  versus  a  spline  input  of  the  endpoint.  Figure  9.6  shows  the  reference 
profiles  given  hub  spline  inputs  while  Figure  9.7  shows  the  reference  given  endpoint 
spline  inputs. 

Notice  in  Figure  9.6  that  while  the  hub  angle  commands  are  the  same  for  a  simul¬ 
taneous  slew  of  -40°  in  3  seconds,  the  Xgp  and  y^p  references  are  actually  different.  In 
this  case,  the  Xgp  reference  reaches  the  desired  position  in  about  2  seconds  while  the 
ygp  reference  takes  about  3  seconds.  Interestingly,  both  Xgp  and  y^p  approach  their  fi- 


nal  positions  in  a  spline  like  manner  for  this  trajectory 


0  1  2  3  4  5  6  7  8  9  10 

t1 

Hub  angtes  vs.  End  Point  Position,  t1=-40,t2=-40 


Figure  9.6-  Hub  Angle  Spline  Input  Reference  Trajectory 


0  .5  1  1.5  2  2.5  3  3.5  4 

t1 

Hub  Angles  vs  End  Point  Position,  End  Point  Slew  (-40,-40) 


Figure  9.7-  Endpoint  Spline  Reference  Trajectory 
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When  commanding  the  same  slew  through  endpoint  positions,  slightly  different 
hub  reference  conunands  are  generated,  as  shown  in  Figure  9.7.  Now,  the  hub  com¬ 
mands  have  slight  overshoot  and  undershoot  in  the  references  and  are  not  ideal  spline 
inputs.  Also,  the  Oj  reference  reaches  the  required  end  angle  in  about  2  seconds  while 
the  02  reference  reached  its  final  value  in  3  seconds. 

9.4.2  Arc 

Another  common  trajectory  which  tests  the  dynamics  of  a  controller  is  a  simple 
circular  arc.  By  specifying  the  start  point,  radius  of  curvature,  arc  length  and  slew 
time,  many  different  variations  can  be  tested.  As  a  start,  an  arc  similar  to  a  physical 
arc  used  in  trajectory  tracking  is  tested.  The  arc  has  a  radius  of  curvature  of  0.51m, 

starts  at  the  0i=O*^  and  02=90°,  and  travels  40°.  Figure  9.8  shows  the  reference  trajec¬ 
tory  for  this  arc  in  terms  of  Xgp  and  ygp. 


x_ep  (m) 

Arc  Ref  TraJ  r=.51m,  40  deg 


Figure  9.8-  40°  Arc  Reference  Trajectory 

This  arc  is  implemented  mathematically  using  the  following  algorithm.  First,  giv¬ 
en  the  starting  point,  radius,  and  arc  length,  the  coordinates  of  the  center  of  the  circle 
corresponding  to  the  arc  are  calculated.  Then,  the  angle  increment  based  on  percent¬ 
age  of  slew  time  is  calculated  to  give  an  angle  position  relative  to  the  start  of  the  arc. 
Knowing  the  angle  position  and  center  of  the  circle,  the  current  endpoint  position' on 
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the  arc  is  calculated.  When  the  time  is  greater  than  the  desired  slew  time,  the  endpoint 
of  the  arc  is  the  reference.  Figure  9.9  shows  a  graphical  depiction  of  the  algorithm. 


CCW=+ 

ti. 


Figure  9.9-  Graphical  Depiction  of  the  Arc  Trajectory 

Mathematically,  the  algorithm  is  defined  by  the  following  equations. 

A  .  fn  Q 

Ax,  =  rsm  --- 


Ayj  =  rcos 


2  2 


2  2) 


Since  the  starting  point  (xj,yj)  is  known,  the  center  point  can  be  found. 


(9.10) 

(9.11) 

(9.12) 

y,  =  yi-Ay  (9.13) 

Once  the  center  point  is  known,  the  trajectory  is  calculated  based  on  the  desired 
slew  time. 


X  =  x,  +Ax 

C  1 


AO  = 


0 


(Tslew) 


(9.14) 


By  knowing  the  change  in  the  arc  angle,  the  change  in  Ax  and  Ay  can  be  calculated 
from  the  following  equation. 

n  0 


Ax  =  rcos 


2  2 


A0 


Ay  =  rsin(l-|-A6) 

Finally,  the  x  and  y  arc  locations  along  the  arc  are  then  known. 


(9.15) 

(9.16) 
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X  =  x^- Ax  (9.17) 

=  +  (9-18) 

When  the  time  is  greater  than  the  slew  time,  the  end  of  the  arc  position  is  used  and  is 
calculated  as  before,  but  with  an  angle  of  {n/2+%/2). 

9.4.3  Sine  Wave 

The  most  challenging  of  the  basic  trajectories  is  the  sine  wave.  Figure  9. 10  shows 
the  sine  reference  trajectory  for  an  amplitude  of  0. 1  m  and  a  length  of  0.25  m. 


x_op  (m) 

Sfna  Ref  TraJ,  AmpI  =  .1m,  Length  =  .25m 

Figure  9.10-  Sine  Wave  Reference  Trajectory 

By  specifying  the  start  position,  amplitude,  length  (period),  and  slew  time,  many 
different  sine  trajectories  are  possible.  A  graphical  depiction  of  the  sine  trajectory  is 
shown  in  Figure  9.11.  Mathematically,  this  trajectory  can  be  produced  for  any  desired 
slew  time  (except  0).  Knowing  the  starting  point  (xi,yi),  the  desired  amplitude  (A), 
the  desired  length  (B),  and  the  slew  time  (Tslew),  a  reference  trajectory  can  be  gen¬ 
erated  from  the  following  equations. 
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Figure  9.11-  Graphical  Depiction  of  the  Sine  Trajectory 

Knowing  the  change  in  x  and  y  allows  calculation  of  the  desired  x  and  y  positions. 

X  =  +  Ax  (9.21) 

y  =  yj  +  Ay  (9.22) 

When  t  >  Tslew,  the  end  of  the  sine  wave  is  simply  yf=yi,  xpxj+B,  and  this  is 

continuously  commanded  at  the  end  of  the  sine  wave. 

9.5  Robust  SANDY  Design 

Robust  controller  design  allows  one  controller  to  perform  over  a  variety  of  plant 
conditions.  For  a  flexible  robotic  manipulator,  two  areas  which  are  important  for  ro¬ 
bustness  are  the  end-point  mass  (and  therefore,  inertia)  and  the  operating  point.  The 
end-point  mass  varies  as  a  payload  is  picked  up  or  dropped,  and  the  system  dynamics 
change.  Also,  the  position  of  the  links  relative  to  each  other  affects  the  dynamics  as 
well.  Providing  plants  with  a  range  of  tip  masses  and  operating  points  allows  an  op¬ 
timization  that  addresses  all  the  given  plant  conditions  simultaneously.  Although  the 
SANDY  controller  is  only  guaranteed  to  work  for  the  specific  plant  cases  included  in 
the  optimization  criterion,  experience  has  shown  that  all  plants  in  between  will  also 
work  [46]. 

For  this  design,  six  different  plant  cases  are  considered,  as  shown  in  Table  9.3. 
There  is  a  low  and  high  mass  plant  at  each  of  the  three  operating  points.  The  plants 
are  linearized  about  each  of  the  operating  points.  For  each  plant  in  Table  9.3,  three 
variations  are  required  to  complete  the  optimization.  These  variations  are  shown  in 


84 


Table  9.4.  For  this  design  problem,  one  plant  variation  is  developed  for  a  filtered  step 
command  input  for  the  first  joint  angle,  and  a  second  plant  applies  a  filtered  step  com¬ 
mand  input  to  the  second  joint  angle.  The  filters  used  for  the  commands  are  first  order 
lags  with  a  time  constant  of  1  second  to  approximate  the  actual  reference  input.  In 
addition,  white  noise  inputs  are  fed  directly  into  the  control  inputs  to  excite  the  high 
frequency  modes  for  the  third  plant  condition.  This  third  plant  is  the  key  to  prevent 
excitation  of  the  flexible  modes.  Overall,  the  total  number  of  plants  optimized  was 
18. 


Table  9.3  -  Robust  Plant  Conditions 


Plant 

Tip  Mass 
kg 

Tip  Inertia 
kg-m^ 

®lnom 

deg 

®2nom 

deg 

1 

.1005 

- MT! - 

0 

2 

1.768 

.0062 

0 

90 

3 

.1005 

.0001 

20 

110 

4 

1.768 

.0062 

20 

110 

5 

.1005 

.0001 

-20 

70 

6 

1.768 

.0062 

-20 

70 

Table  9.4  -  Optimization  Plant  Variations 


Variation 

Purpose 

1 

Optimize  for  error 

2 

Optimize  for  02  error 

3 

Optimize  for  high  frequency 
(Noise  into  both 
control  inputs) 

The  criterion  variables  are  0] ,  02 ,  error_0},  and  error_02,  respectively.  There  are 
two  controls  (torque_l  and  torque_2)  and  two  sensors  (0}  and  02).  The  criterion  vari¬ 
ables  are  used  only  in  the  cost  function,  and  can  contain  variables  other  than  those 
available  from  sensors.  Thus,  the  SANDY  controller  should  have  two  inputs  (the  sen¬ 
sors)  and  two  outputs  (the  torques).  Values  of  the  nonlinear  constraints  are  a  direct 
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constraint  on  the  control  covariance  of  torque  1  for  plant  1  of  10  Nm  and  a  constraint 
on  the  maximum  real  part  for  the  closed-loop  eigenvalue  of  -0.2.  Also,  no  weighting 
is  applied  to  the  control  penalties  Rj  and  R2,  and  Qj=  diagonal(0,0,l,0)  with  Q2  = 
diagonal(0, 0,0,1).  The  initial  guess  of  the  SANDY  controller  is: 


0  0  0  0 

1  0 

A  = 

0  0  0  0 

,B  = 

0  1 

,C  = 

0.1  0  8  0 

,D  = 

2  0 

0  0-80 

1  0 

_0  0.11  0  9 

_0  2.1_ 

00  0  -9 

0  1 

(9.23) 


This  is  a  fourth-order  controller  with  2  integral  states  and  a  first-order  filter  for  the  0j 
and  02  sensor  inputs,  and  is  similar  to  the  PID  controller  described  in  a  previous  chap¬ 
ter.  The  integral  states  are  added  to  reduce  steady-state  error  in  tracking  the  joint  an¬ 
gles.  The  optimized  variables  are  A(3,3),  A(3,4),  A(4,3),  A(4,4),  B(3,l),  B(3,2), 
B(4,l),  B(4,2),  C(l,3),  C(l,4),  C(2,3)  and  C(2,4)  for  the  first-order  filters.  In  addition, 
C(1,1),C(1,2),C(2,1),  and  C(2,2)  are  the  design  variables  for  the  integral  gains.  Final¬ 
ly,  the  entire  D  matrix  is  optimized  for  the  proportional  gains. 

The  final  design  weightings  for  each  plant  case  are  chosen  as: 


0  0  0  0 

0  0  0  0 

0  0  0 

0 

0  0  0  0 

,Q2  = 

0  0  0  0 

,03  = 

00  0 

0 

0  0  5  0 

0  0  0  0 

0  0  0.1 

0 

0  0  0  0_ 

0  0  0  20_ 

0  0  0 

0.1 

(9.24) 


while  no  weightings  were  applied  to  the  control  penalty  matrices.  In  addition,  a  con¬ 


straint  on  the  maximum  real  part  of  the  closed-loop  eigenvalues  was  applied  to  each 


plant.  This  maximum  real  value  of  the  roots  was  -0.2,  and  forced  the  integral  poles 
away  from  the  imaginary  axis. 


After  2454  iterations,  the  resulting  SANDY  controller  converged  to  the  following 
solution: 


0  0 

0 

0 

1 

0 

0  0 

0 

0 

,B  = 

0 

1 

0  0 

-1.56 

-22.23 

4.97 

6.04 

0  0 

0.63 

-5.18 

1.75 

-4.85_ 

(9.25) 
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c  = 

0.68 

1.71  4.64  24.40 

,D  = 

0.76  -8.82 

0.38 

1.40  0.60  9.9 1_ 

0.92  9.88  _ 

The  total  CPU  time  used  was  3  hours  and  31  minutes  on  a  Digital  Equipment  Corpo¬ 
ration  alpha  machine. 

The  bandwidth  plots  (including  the  1st  order  command  prefilter  at  1  rad/s)  are 
shown  in  Figure  9.12  and  Figure  9.13.  Once  again,  the  filtered  plots  are  shown  since 
that  is  what  is  actually  fed  into  the  arm  and  since  that  same  filter  value  was  used  in 
the  SANDY  optimization.  The  filtered  shoulder  motor  bandwidth  is  about  1.25  rad/ 
s  while  the  elbow  motor  bandwidth  is  about  1.1  rad/s.  Compared  to  the  original 
SANDY  design,  the  shoulder  motor  bandwidth  is  about  0.25  rad/s  higher  with  the  ro¬ 
bust  SANDY  design. 


Frequency  [rad/sec] 

SANDY  Robust  4th  Order  (pref=1 ),  th1  -th1_cmd  BW 

Figure  9.12-  Filtered  Shoulder  Bandwidth,  Robust  4th  order  SANDY 

The  controller  is  discretized  using  a  Tustin  transformation  at  a  1  ms  sampling 
time.  The  difference  between  a  1  ms  simulation  and  a  continuous  simulation  was  neg¬ 
ligible  for  this  controller,  verifying  the  initial  assumption  that  a  continuous  SANDY 
design  should  provide  a  good  digital  controller  at  this  1000  Hz  sampling  rate.  If  dig- 
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ital  considerations  are  desired,  more  stringent  constraints  may  be  placed  ,  on  the 
control  covariances  to  keep  the  gain  on  the  controller  low  making  it  more  applicable 
for  higher  sampling  times  (and  thus  lower  sampling  rates).  Additionally,  a  direct  dig¬ 
ital  synthesis  method  using  SANDY  is  being  developed. 


Frequency  [rad/sec] 

SANDY  Robust  4th  Order  (pref=1),  th2-th2_cmd  BW 


Figure  9.13-  Filtered  Elbow  Bandwidth,  Robust  4th  order  SANDY 
9.6  Experimental  Comparison 

On  all  the  experimental  plots  showing  volts,  the  conversion  factor  to  Nm  is  1/ 
1 .9208  for  tj  and  1/8.045  for  T2-  Please  note  that  all  plots  in  this  paper  have  time  units 
of  seconds  and  have  angle  outputs  measured  relative  to  the  starting  position. 

The  medium  tip  mass  nominal  configuration  was  tested  first  for  all  three  end¬ 
point  tasks.  All  plots  compare  the  4th-order  Robust  SANDY  and  20th-order  LQQ 
controllers  to  the  reference  command.  Figure  9.14  shows  a  3  sec  line  slew  corre¬ 
sponding  to  about  -20  degree  changes  in  both  angles  simultaneously.  In  this  plot,  the 
4th-order  SANDY  controller  matches  the  reference  line  slightly  worse  than  the  20th- 
order  LQG  controller. 

Figure  9.15  shows  the  same  reference  line,  but  allows  10  seconds  to  perform  the 
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Another  end-point  task  is  tracking  an  arc.  In  this  case,  a  40  degree  arc  with  a  radius 
of  curvature  of  0.51  inches  is  tracked  in  3  seconds.  Notice  that  the  LQG  controller 
tracks  much  more  accurately  for  this  case,  as  shown  in  Figure  9.16. 


Arc  (10  Sec),  Exp  LQG,  SANDY,  Ref,  rs.SI",  theta  =  40  deg 


Figure  9.17-  Exp  10  Sec  Arc:  LQG,  SANDY,  Ref  for  R=.51”,  0=40  deg 
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The  reason  for  the  poor  SANDY  performance  in  this  case  is  the  low  gains  for  the  first 
motor  and  the  relatively  little  amount  of  traverse  required  for  the  first  motor  by  this 
trajectory.  Thus,  only  small  torques  are  required  and  they  don’t  overcome  the  resid¬ 
ual  friction  in  the  motor  shaft  initially.  Note  that  the  steady-state  error  is  lower  for  the 
SANDY  controller,  though. 

In  Figure  9.17,  notice  that  the  performance  between  the  controllers  is  much  clos¬ 
er,  even  though  the  LQG  controller  still  does  a  slightly  better  job.  Thus,  the  tracking 
performance  for  both  controllers  increases  as  the  task  is  done  more  slowly. 

Figure  9.18  shows  that  both  controllers  have  trouble  tracking  a  sine  wave  with  an 
amplitude  of  0.1  m  and  a  length  of  0.25  m  in  3  seconds.  The  SANDY  controller  pro¬ 
duces  more  overshoot,  but  the  tracking  trends  are  close  for  both. 


Figure  9.18-  Exp  6  Sec  Sine:  LQG,  SANDY,  Ref  for  Amp=:.lm,  Length=.25m 

By  increasing  the  tracking  time  to  20  seconds,  significant  improvement  for  both 
controllers  occurs.  The  SANDY  controller  initially  has  several  small  oscillations,  but 
has  lower  steady-state  error.  Figure  9.19  shows  the  results  of  this  trajectory. 


x_ep 

Line  (10  Sec),  Exp  LOG,  SANDY,  Ref,x,y:.64,.61  -  .99, .25,  Light  Tip 

Figure  9.20-  Exp  10  Sec  Line:  LQG,  SANDY,  Ref  from  x,y=.64,.61  -  .99.25  m, 

Light  Tip 


The  different  trajectories  test  the  robustness  to  arm  position,  but  not  to  payload 
uncertainty.  Therefore,  tests  are  run  with  the  light  tip  and  a  heavy  tip  used  in  the  ro- 
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bust  SANDY  design.  The  same  line  trajectory  is  run  in  10  seconds  with  the  light  tip 
and  the  results  are  shown  in  Figure  9.20.  In  this  case,  both  controllers  are  comparable. 


Arc  (10  Sec),  Exp  LQG,  SANDY,  Ref,  r:=51",  theta=40  deg,  Light  Tip 

Figure  9.21-  Exp  10  Sec  Arc:  LQG,  SANDY,  Ref  for  R=.51”,  6=40  deg.  Light  Tip 


x_ep 

Sin  (20  Sec),  Exp  LQG,  SANDY,  Ref,  Amp=.1m,  Length=.25m,  Light  Tip 


Figure  9.22-  Exp  20  Sec  Sine:  LQG,  SANDY,  Ref  for  Amp=.lm,  Length=.25m, 

Light  Tip 


93 


When  the  arc  trajectory  is  run  in  10  seconds  with  the  light  tip,  the  LQG  controller 
still  provides  better  performance  than  the  SANDY  controller,  as  shown  in  Figure 
9.21. 

The  same  trends  as  with  the  medium  tip  occur  for  the  20  second  sine  trajectory. 
Figure  9.22  shows  that  both  controllers  provide  comparable  performance 

Switching  to  the  heavier  tip  with  a  ball  and  cup  provides  a  payload  test  in  the  op¬ 
posite  direction  to  the  light-weight  tip.  Figure  9.23  shows  that  for  the  10  sec  line 
trajectory,  the  SANDY  controller  provides  better  performance  than  the  LQG  control¬ 
ler.  In  fact,  the  SANDY  controller  shows  little  degradation  for  this  trajectory  despite 
speed  and  payload  variations. 


Line  (10  Sec),  Exp  LQG,  SANDY,  Ref,x,y:.64,.61  -  .99,.25,  Heavy  Tip 

Figure  9.23-  Exp  10  Sec  Line:  LQG,  SANDY,  Ref  from  x,y=.64,.61  -  .99.25  m, 

Heavy  Tip 

For  the  10  second  arc  trajectory  with  the  heavy  tip,  the  LQG  performance  actually 
improves,  while  the  SANDY  controller  degrades,  as  shown  in  Figure  9.24. 

Finally,  the  performance  for  both  controllers  is  fair  for  the  20  second  sine  trajec¬ 
tory,  which  Figure  9.25  shows.  The  LQG  controller  has  a  slight  edge  here,  as  the 
SANDY  controller  initially  has  a  lag  in  following  the  sine  wave  with  a  heavier  mass. 


probably  due  to  the  low  first  motor  control  gains  again 


Stn  (20  Sec),  Exp  LQG,  SANDY,  Ref,  Amp=,1m,  Length=.25m,  Heavy  Tip 

Figure  9.25-  Exp  20  Sec  Sine:  LQG,  SANDY,  Ref  for  Amp=.lm,  Length=.25m, 

Heavy  Tip 
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Overall,  the  SANDY  controller  provides  better  performance  for  the  line  trajecto¬ 
ry,  the  LQG  controller  performs  better  for  the  arc  trajectory,  and  both  are  about  the 
same  for  the  sine  trajectory.  Both  controllers  proved  robust  to  payload  and  arm  posi¬ 
tion,  but  both  exhibited  degraded  performance  when  the  tracking  speed  increased. 

Appendix  D  contains  additional  plots  for  varying  speeds  along  the  line  and  arc 
trajectories.  The  performance  improved  as  the  speed  decreased,  and  degraded  as  the 
speed  improved. 


10  Hybrid  Position  and  Force  Control  Overview 

A  challenging  real-world  application  for  a  flexible  manipulator  is  applying  a  con¬ 
stant  force  to  a  surface.  Examples  of  this  application  include  buffing,  grinding, 
polishing,  or  even  painting  surfaces.  Accomplishing  such  tasks  requires  an  algorithm 
which  controls  both  position  and  force  simultaneously. 

Humans  apply  position  control  with  force  feedback  when  trying  to  accomplish 
such  tasks  as  placing  a  peg  in  a  hole.  The  eye  provides  an  estimate  of  the  position, 
and  the  nerves  in  the  hand  and  arm  act  as  force  sensors.  For  a  robotic  manipulator, 
the  “eyes”  of  the  system  are  the  position  sensors  and  the  “nerves”  of  the  system  are 
the  components  of  force  sensed  from  a  6-component  force/torque  sensor.  When  hu¬ 
mans  follow  a  surface,  the  eyes  allow  them  to  place  a  hand  onto  a  surface,  and  the 
nerves  provide  feedback  as  to  how  much  force  is  applied  to  the  surface.  In  the  same 
manner,  a  robot  can  use  both  force  and  position  information  to  follow  a  surface. 

For  a  rigid  surface,  like  metal  or  wood,  the  force  and  position  controllers  can  be 
decoupled  [34].  The  force  control  is  used  to  apply  torques  in  the  normal  direction, 
which  is  also  known  as  axial  or  longitudinal  control.  The  position  control  operates  on 
the  tangential  or  lateral  direction.  Thus,  the  two  controllers  can  be  decoupled  and  de¬ 
signed  separately.  The  overall  hybrid  control  is  thus  position  control  along  the 
surface  tangent  and  force  control  along  the  surface  normal. 

10.1  Force  Control 

One  of  the  two  required  tasks  for  surface  following  is  force  control.  The  lumped- 
spring-mass-damper  model  used  for  position  control  does  not  address  any  of  the 
force  characteristics  of  the  TLA.  Therefore,  a  force  model  is  required.  Additionally, 
a  way  to  generate  the  desired  force  reference  commands  is  required.  Before  the  force 
model  is  developed  and  the  algorithm  explained,  some  basic  concepts  behind  force 
control  are  needed. 

10.1.1  Jacobian 

It  is  appropriate  to  describe  the  Jacobian  as  it  relates  to  robotic  applications  [15]. 
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Given  a  set  of  equations 

Y  =  F{X)  (10.1) 

where 

yi  =  ...,Xn)  (10.2) 

and 

Y  =  [y^,y2,  (10.3) 

To  calculate  the  differentials  of  yj  as  a  function  of  the  differentials  of  Xj,  use  the 

chain  rule  to  obtain 


X  X  X  X 


df~  9/-  9/- 

6y.,  =  TIT — 8x,  +Tr — bx~  +  ...  +.^^5x 
^  9xj  *  9^2  ^  ox^  ” 

df  9/  9/ 

Sjn  =  +3^8X2+  ...  +3^8x 

9xj  1  9^2  2  9x^  " 


(10.4) 


which  simplifies  to 


8y  =  (10.5) 

dX 

The  matrix  of  partial  derivatives  is  known  as  the  Jacobian,  denoted  by  J.  Thus, 
another  expression  which  is  equivalent  to  Equation  (10.5)  is 

dY  =  JiX)bX  (10.6) 

For  the  TLA  shown  in  Figure  10.1,  the  Jacobian  becomes 


(-f/jSinOj -f/2sin  (0j  +  62) )  (-i/2sin  (0j  +  62) )  80j  ^2q.7^ 

(9jCos0j  +  <i2Cos  (0j  +  02) )  (0i  +  O2)  ^^2 

Notice  that  the  Jacobian  varies  with  the  manipulator  position.  For  example,  the 

mapping  between  angles  and  end-point  is  quite  different  between  the  (0°,90°)  con¬ 
figuration  and  the  (0®,0®)  configuration,  where  the  numbers  in  parenthesis  are 
(01,02).  At  the  first  condition,  a  small  change  in  02  is  almost  exclusively  a  change  in 
X,  while  at  the  second  condition  a  small  change  in  02  is  almost  exclusively  a  change 
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in  y. 


Figure  10.1-  Jacobian  Parameters  for  the  TLA 


10.1.2  Principle  of  Virtual  Work 

The  relationship  between  torque  and  force  is  critical  for  force  control  since  the 
force  is  controlled  through  the  joint  torques.  When  forces  act  on  a  device,  work  is 
done  if  the  device  goes  through  a  displacement  [15].  By  definition,  work  is  force  act¬ 
ing  though  a  distance.  The  principle  of  virtual  work  allows  relationships  for  the  static 
case  as  the  displacement  goes  to  an  infinitesimally  small  amount.  For  a  multi-link 
manipulator,  this  principle  may  be  written  in  equation  form  as 

T^bQ-F^bx  =  0  (10.8) 

where  for  the  TLA 


(10.9) 

(10.10) 


(10.11) 


and  T  is  a  vector  of  joint  torques,  60  is  a  vector  of  joint  angles,  and  6x  is  a  vector  of 
end-point  position  coordinates. 

For  the  TLA,  we  can  write  the  relationship  between  the  end-point  and  the  angles 
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as 

8jc  =  750  (10.12) 

10.1.3  Identification  of  the  Force  Transfer  Functions 

Before  design  and  simulation  of  a  force  controller  may  begin,  a  model  is  required. 
One  experimental  method  which  may  identify  the  force  transfer  functions  is  Approx¬ 
imate  Linearization  [34].  Using  the  Jacobian,  one  can  calculate  the  joint  torques 
necessary  to  provide  a  desired  end-point  force  for  the  case  of  static  equilibrium.  Thus, 
feeding  a  sinusoid  into  a  joint  while  positioning  the  end-point  at  a  solid  object  allows 
for  a  sinusoidal  application  of  force  at  the  end-point.  Thus,  a  frequency  response 
curve  for  torque  at  joint  1  to  the  axial  force  as  well  as  torque  at  joint  2  to  the  tangential 
force  can  be  obtained.  From  the  frequency  response,  a  modal  representation  of  the 
system  may  be  obtained  in  the  form  of  a  transfer  function  which  matches  the  exper¬ 
imental  frequency  response.  Figure  10.2  shows  a  graphical  depiction  of  the  method. 


Approximately  Linear  System 


I _ I 


Figure  10.2-  Identification  by  Approximate  Linearization 

With  an  understanding  of  the  Jacobian  and  the  Principle  of  Virtual  Work,  the  Ap¬ 
proximate  Linearization  makes  sense.  To  verify  the  position  model,  a  torque  is 
applied  as  a  sine  wave  to  the  motor  at  varying  frequencies.  A  transfer  function  be¬ 
tween  the  commanded  angle  and  measured  angle  is  obtained  at  different  frequencies 
to  determine  the  frequency  response.  Since  torque  is  related  to  force  through  the  Ja¬ 
cobian  [34],  it  makes  sense  that  a  frequency  response  for  force  should  be  obtained  in 
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a  similar  manner  to  the  position  transfer  function,  which  depends  on  the  torque  di¬ 
rectly.  Another  way  to  think  of  the  relationship  between  the  position  and  force 
transfer  functions  is  that  the  position  transfer  function  corresponds  to  an  end-point 
which  is  unpinned,  while  the  force  transfer  function  corresponds  to  an  end-point 
which  is  pinned. 

10.1.4  Force  Components  and  Coordinates 

Since  the  force  controller  is  outfitted  with  a  low-friction  roller,  essentially  the 
only  force  sensed  at  the  surface  is  the  normal  component  of  force  [34].  There  is  a  re¬ 
lationship  between  the  surface  normal  and  the  surface  tangent  force,  which  is  shown 
in  Figure  10.3. 


Figure  10.3-  Surface  Force  Diagram 

The  surface  normal,  which  is  the  sensed  force,  has  both  x  and  y  components. 
These  are  related  to  the  tangential  direction  components  through  similar  triangles. 
Both  can  be  represented  in  equation  form  [34]. 
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F=fJ+fJ  (10.13) 

where  F  is  the  surface  normal  force.  Additionally,  the  tangential  direction  vector  T 
can  be  represented  as 

T  =  tJ  +  tJ  (10.14) 

Finally,  the  tangential  components  are  related  to  the  normal  components  by  the  fol¬ 
lowing  equation 


T  = 


4 

ifi 


'i-l 


fx 

l^’l 


(10.15) 


Another  consideration  when  examining  force  components  is  the  reference  frame. 
The  desired  forces  are  normally  considered  in  the  inertial,  or  fixed  reference  frame. 
The  frame  which  is  associated  with  the  force  transducer  is  defined  along  the  second 
link. 


This  frame  is  called  the  measured  frame,  since  the  force  measurements  take  place 
here.  Figure  10.4  shows  the  relationship  between  the  two  frames.  Notice  the  relation¬ 
ship  between  the  two  frames  is  simply  a  function  of  the  combination  of  the  first  and 


second  link  angles.  Mathematically,  the  relationship  is  expressed  as: 
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/x  = /imSin  (0o)  (60) 

and 

(10.16) 

fy  =  (Go)  +/2^sin  (Gq) 

where 

(10.17) 

^0  “ 

(10.18) 

The  conversion  from  the  inertial  frame  to  the  measured  frame  is  also  needed.  The 

equations  are: 

/xm  =/xSineo-/>,cosGo 

and 

(10.19) 

4m  =/xCos0o+//inGo 

(10.20) 

10.1.5  Force  Control  Structure 

With  knowledge  of  the  basic  concepts  covered  in  the  previous  force  sections,  a 
force  control  scheme  becomes  plausible.  Figure  10.5  shows  a  block  diagram  of  the 
scheme.  In  words,  the  x  and  y  components  of  force  arm  are  obtained  in  the  measured 
frame,  and  a  magnitude  is  calculated.  Using  the  angle  positions  for  the  links,  a  con¬ 
version  to  the  inertial  frame  is  accomplished.  Once  in  the  inertial  frame,  the  normal 
components  for  force  are  calculated.  These  components  can  be  thought  of  as  direc¬ 
tions  of  the  force,  and  are  combined  with  the  desired  magnitude  to  produce  reference 
commands  for  both  the  x  and  y  components  of  force  in  the  inertial  frame.  Since  the 
actual  forces  are  obtained  in  the  measured  frame,  the  reference  conunands  are  con¬ 
verted  to  the  measured  frame.  An  error  signal  in  both  components  of  force  is 
produced,  and  fed  into  the  force  controller.  The  resulting  force  commands  are  then 
converted  to  joint  torque  commands  through  the  Jacobian.  It  is  these  joint  torque 
commands  that  are  fed  to  motors  to  move  the  arm. 

The  objective  of  this  portion  of  the  design  is  determine  a  controller  which  pro¬ 
vides  high  bandwidth  force  control  without  exciting  any  of  the  arm  flexible  modes. 
The  same  general  scheme  is  used  regardless  of  the  specific  controller. 
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Figure  10.5-  Force  Control  Scheme 

10.2  Position  Control 

For  a  rigid  surface,  the  lateral  or  tangential  direction  is  primarily  controlled  by  the 
position  controller.  In  addition,  the  position  controller  is  the  only  controller  used  to 
move  the  arm  until  contact  is  made  with  a  surfaee. 
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10.2.1  Position  Control  With  Surface  Knowledge 

In  many  industrial  applications,  the  surface  shape  is  known.  In  this  case,  the  po¬ 
sition  control  scheme  is  almost  exactly  like  that  described  in  Chapter  9.  The  reference 
trajectory  in  terms  of  end-point  is  generated  to  match  the  trajectory  from  the  starting 
position  of  the  arm  to  the  surface.  Next,  the  surface  contour  coordinates  are  used  as 
the  position  reference  for  the  remainder  of  the  trajectory.  Essentially,  the  x  and  y 
splines  are  replaced  by  a  more  complex  trajectory  generator,  but  everything  else  re¬ 
mains  the  same  for  position  control. 

10.2.2  Position  Control  Without  Surface  Knowledge 

The  most  challenging  control  application  for  surface  following  with  force  control 
is  to  follow  a  surface  without  knowledge  of  the  surface.  One  surface  following  algo¬ 
rithm  which  tackles  this  problem  was  used  by  Kraft  [34].  Figure  10.6  shows  a  picture 
of  this  algorithm. 


Figure  10.6-  Surface  Following  Algorithm 
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The  surface  following  algorithm  can  be  described  both  in  words  and  by  equation. 
In  words: 

1 .  Define  the  end-point  to  be  at  point  Pj  at  time  tQ. 

2. a.  Define  a  position  in  Cartesian  coordinates  (Pxci>Pyci)-  These  coordinates 
are  located  a  distance  k  along  the  tangent  direction. 

2. b.  At  the  same  time  as  2.a.,  a  force  in  the  direction  opposite  the  surface  nor¬ 
mal  is  commanded. 

3 .  At  time  tj ,  one  sample  period  after  tQ,  the  end-point  is  at  P2  having  the  Car¬ 
tesian  coordinates  (Px,py).  The  new  surface  normal  and  tangent  directions  are 
calculated  at  this  new  position. 

4.  The  previous  commanded  position,  (Pxci>Pyci)’  is  then  projected  onto  a  line 
that  is  parallel  to  the  new  surface  tangent  and  passes  through  P2.  This  defines  the 
point  P3. 

5. a.  The  next  commanded  position,  (Pxc2,  Pyc2)>  is  then  located  a  distance  k 
from  P3  along  the  new  tangent  direction. 

5.b.  Again,  a  force  in  the  direction  opposite  the  current  surface  normal  is  com¬ 
manded  at  the  same  time  as  5. a.. 

This  process  is  repeated  every  sample  period.  The  resulting  surface  tracking  velocity 
can  then  be  expressed  as 

'^tracking  ~  j  (10.21) 

where  T  is  the  sample  period. 

The  algorithm  may  also  be  stated  in  a  more  mathematical  form.  Defining  the  nor¬ 
mal  and  tangential  forces  in  Equations  (10.13)-  (10.15)  is  the  starting  point  for  the 
mathematical  description.  Next,  let  (Px,Py)  be  the  Cartesian  coordinates  of  the  current 
end-point  position,  and  let  (pxci.Pyd)  be  the  Cartesian  coordinates  of  the  last  com¬ 
manded  position.  Then  the  projection  of  the  last  commanded  position  on  a  line 
passing  through  (px,Py)  and  parallel  to  t  is 

^  ^  ^Pxc\-Px)tx^  iPycl-Py)ty 


(10.22) 
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Also,  the  Cartesian  coordinates  of  the  next  commanded  position,  (pxc2,pyc2),  are 


given  by 

Pxc2=  + 

(10.23) 

and 

Pyc2  =  Py-^  + 

(10.24) 

where  k  is  a  constant. 


10.3  Hybrid  Control  Structure 

The  hybrid  control  structure  involves  torques  from  the  position  controller  and  the 
force  controller.  Figure  10.7  shows  a  block  diagram  of  the  hybrid  scheme. 
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Figure  10.7-  Hybrid  Force  and  Position  Control  Scheme 

It  is  important  to  realize  that  in  this  scheme,  the  two  controllers  may  oppose  each 
other  in  some  circumstances.  For  example,  if  the  commanded  position  is  away  from 
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the  surface,  then  the  position  controller  will  try  to  move  the  end-point  away  from  the 
surface.  At  the  same  time,  the  force  controller  will  try  to  maintain  a  constant  force  by 
moving  the  end-point  into  the  surface.  Thus,  the  two  controllers  are  both  trying  to  ac¬ 
complish  the  opposite  effect!  One  way  to  reduce  this  effect  is  to  bandwidth-separate 
the  controllers  so  that  one  controller  tends  to  dominate  the  response  [34].  In  this  case, 
force  control  should  have  a  higher  bandwidth  than  the  position  controller. 

One  other  insight  into  this  particular  application  is  that  generally,  the  position 
controller  will  control  the  lateral  direction  and  the  force  controller  will  control  the 
longitudinal  direction,  as  stated  earlier.  Thus,  if  the  surface  is  known,  the  position 
command  will  generally  not  command  the  end-point  to  leave  the  surface.  For  the  un¬ 
known  surface  case,  the  algorithm  should  command  a  direction  along  the  tangent  and 
not  into  surface.  Therefore,  conditions  where  the  two  controllers  are  fighting  each 
other  should  be  minimized. 

10.4  Task  Overview 

The  task  overview  can  be  stated  as: 

To  develop  robust  reduced-order  position  and  force 
controllers  that  allow  surface  following  of  an  unknown 
surface  while  maintaining  a  constant  contact  force. 

The  control  task  starts  from  any  arbitrary  initial  loca¬ 
tion  of  the  flexible  link  manipulator  and  uses  position 
control  until  touchdown  on  the  object,  then  uses  hybrid 
position  and  force  control  to  perform  the  surface  fol¬ 
lowing.  Finally,  the  task  ends  by  returning  to  the  initial 
arbitrary  starting  location. 

This  represents  a  typical  industrial  application  such  as  polishing  or  grinding. 
Once  the  framework  is  designed,  the  task  is  to  design  the  best  controllers.  Figure  10.8 
shows  a  diagram  of  the  surface  following  task.  Notice  that  task  requires  movement 
of  the  end-point  in  two  distinct  phases  -  pure  position  movement  from  1-2,  2-3  and 
4-1,  and  then  hybrid  force  and  position  control  from  3-4  while  following  the  surface. 
The  most  critical  part  of  the  task  is  the  touchdown  on  the  surface,  which  occurs  at 
point  3.  The  control  of  the  end-point  is  achieved  through  movement  of  the  actuators 
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for  links  1  and  2. 

Since  this  is  a  flexible  manipulator,  the  end-point  position  can  only  be  approxi¬ 
mated  through  forward  kinematics.  However,  the  force  control  uses  a  non-collocated 
sensor,  namely  the  force  sensor.  Thus,  the  end-point  position  need  not  be  known  ex¬ 
actly  since  the  force  sensor  compensates  for  any  uncertainty. 


Figure  10.8-  Surface  Following  Task 

Currently,  two  surfaces  are  available  as  test  cases  for  the  surface  following  tasks. 
These  surfaces  are  shown  in  Figure  10.9.  The  simplest  surface  to  follow  is  a  straight 
line,  hence  this  target  provides  a  baseline  to  test  the  controllers  initially.  The  arc  is 
more  challenging  since  it  requires  some  interaction  between  the  two  links,  and  can 
be  placed  in  either  a  concave  or  a  convex  position.  It  most  closely  matches  a  trajec¬ 
tory  which  might  be  used  in  industry.  In  particular,  at  the  top  of  the  semicircles,  the 
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controllers  must  provides  a  means  to  maintain  contact  even  after  previous  movement 
of  the  end-point  in  a  trajectory  which  might  lead  off  the  surface. 


25’’  LINE 


11  Force  Control  Model  and  Design 


The  ultimate  goal  of  this  research  is  to  provide  path  following  for  a  two-link  flex¬ 
ible  robotic  manipulator  through  the  use  of  hybrid  position  and  force  control.  Since 
these  controllers  act  in  perpendicular  directions  for  a  rigid  surface  [34],  it  is  logical 
to  design  them  separately.  The  position  control  designs  are  discussed  in  previous 
chapters,  so  this  chapter  will  focus  on  the  design  of  force  controllers.  Before  the  force 
controllers  can  be  designed,  however,  force  models  are  needed  for  both  the  x  and  y 
directions. 


11.1  Force  Models 

The  force  models  are  based  on  the  approximate  linearization  technique  used  by 
Kraft  [34].  This  is  an  empirically-based  technique  which  maps  the  motor  torques  to 
forces  through  the  Jacobian,  as  shown  in  Chapter  10.  Sine  sweeps  are  done  separately 
for  the  x-force  and  y-force.  When  conducting  the  y-force  (normal  force)  test,  the  sen¬ 
sor  is  brought  in  contact  with  surface  perpendicular  to  the  y  direction.  In  a  like 
manner,  the  x-force  (side  or  tangential  force)  test  is  conducted  against  a  surface  per¬ 
pendicular  to  the  X  direction.  Then,  the  frequency  responses  of  the  tests  are  matched 
to  mathematical  models  for  both  axes,  and  thus  two  force  models  are  obtained.  For 
this  work,  both  the  x  and  y  models  are  11th  order  single-input  single-output  transfer 
functions.  The  model  consists  of  a  filter  combined  with  different  modes  to  best  match 


the  frequency  response.  In  addition,  there  is  a  noise  filter  at  10  rad/s  on  the  force 
transducer  which  is  implemented  in  the  data  acquisition  software. 

The  transfer  function  for  the  Fy  model  is: 
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11.2  Force  Control  Designs 

As  a  basis  of  comparison,  standard  linear-quadratic-gaussian  (LQG)  and  propor¬ 
tional-integral  (PI)  controllers  are  used.  Next,  the  LQG  controllers  are  reduced 
through  both  balanced  truncation  and  modal  residualization.  The  key  force  control 
design  method  is  the  SANDY  direct  optimization  program.  Using  SANDY  allows 
the  design  of  robust  reduced-order  force  controllers,  which  is  another  key  thrust  of 
this  dissertation.  Since  there  is  an  x  and  y  force  model,  separate  force  controllers  must 
be  designed  for  each  direction. 


11.2.1  PI  Force  Control 

One  classical  control  approach  which  is  used  due  to  its  simplicity  is  PI  control. 
For  this  application,  the  PI  control  gains  were  iteratively  tuned  in  simulation  and  then 
experimentally.  The  Fx  controller  uses  gains  of  PI=.3,5  and  the  Fy  controller  uses 
gains  of  PI=.7,6.  Figure  11.1  shows  the  experimental  and  simulation  results  for  the 
Fx  controller  match  fairly  well.  Similarly,  Figure  1 1.2  shows  the  same  results  for  the 
Fy  controller  which  also  displays  a  close  match  between  experimental  and 
simulation. 


11.2.2  LQG  Force  Control 

For  this  design,  the  LQG  controller  uses  the  same  general  procedure  as  described 
for  LQG  position  control.  In  this  case,  however,  there  are  two  distinct  controllers  - 
one  for  Fx  and  one  for  Fy.  Also,  there  is  1  reference  input,  1  sensor  input,  and  one 
control  output.  Both  Fx  and  Fy  models  are  1 1th  order,  with  a  noise  filter  in  each  sen¬ 
sor  loop  located  at  10/(s+10).  Thus,  the  plant  is  effectively  12th  order.  Additionally, 
an  integral  control  state  is  added  to  both  controllers  to  reduce  steady  state  error, 
Therefore,  both  the  LQG  Fx  and  Fy  controllers  are  13th  order. 

First,  the  Fx  LQR  criterion  is  simply  the  error  in  Fx  and  the  control  is  simply  ufx. 
So,  the  LQR  weightings  for  the  Fx  controllers  are  Q=40  and  R=.075.  The  LQE  esti- 
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mator  assumes  no  process  noise,  except  for  a  small  amount  at  the  control  input.  Also, 
the  sensor  is  the  Fx  measurement. 


Figure  11.1-  Experimental  vs.  Sim,  PI=.3,5 ,  Fx  Controller 


Exp  vs.  Sim,  PI  =.7, 11=6, 25gf-50gffy  command, (step) 


Figure  11.2-  Experimental  vs.  Sim,  PI=.7,6 ,  Fy  Controller 
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The  weightings  are  W=[.01]  and  V=:[l]  for  this  design.  The  resulting  LQG  con¬ 
troller  is; 

-18.7  -12.4  -1.8  -9.2  -3.9  -7.9  -4.5  -8.7  -5.2  -14.7  -16.6  -8  92.4' 

-10  -154.4-192  -83.4-165.6  -49.2  -92.5  -30.1  -43.7  -43  -14  -9.3  0 

0  1024  0  0  0  0  2e-6  5Ae~6  l.le-5  6.9e~5  4.6e-5  3Ae-5  0 

0  0  256  0  •  0  1.3e-5  3.3e-5  9Ae-5  \.9e-4  l.2e-3  7.6e-4  5Ae-4  0 

0  0  0  256  0  ~le-5  -2.5e-5 -7.0e-5  -  1.4e-4 -8.9e-4  ™5.9e-4 -4.0e-4  0 

0  0  0  0  256  ~\Je-4-43e-4-l.2e-3  2Ae~3  -1.5e-2 -9.9e-3 -6.6c-3  0 

^=  0  0  0  0  0  128  1.6e-4  4Ae-4  9Ae-4  5.6e~3  3Je-3  2.5e-3  0  (11.3) 

0  0  0  0  0  5.7^-4  128  3.9e-3  8^-3  5^-2  3.3^-2  2,2^-3  0 

0  0  0  0  0  -9.6e-5 -2.4e-4  64  -l.3e-3  8.3e-3  -5.5e-3 -3.7e-3  0 

0  0  0  0  0  -2.5c-4-6.3e-4-1.7^-3  32  -2.2e-2  -  1.5e - 2  9.Se~3  0 

0  0  0  0  0  -9.7e-5-2.4e-4-6.6e-4-1.4e-3  16  -5.6e-3 -3.8e- 3  0 

0  0  0  0  0  -7e-5  _  1.7^-4 -4.7e-4 -9.7e-4  -6e-3  4  ~2Je-3  0 

_000000  0  0  0  0  0  0  0_ 

6.7e^3  0 
7Je-6  0 
-23e-5 0 
-3.8e-4  0 
2.9e-4  0 
4.9e-3  0 
B=  -\,9e-3 
^\.6e-2  0 
2.7e-3  0 
7.3^ -3  0 
23e-3  0 
2.0e-3  0 
-1  1_ 

C  =  [-2.2  -3.1  -0.45  -2.3  -0.97  -2.0  -1.1  -2.2  -1.3  -3.7  -4.1  -2.0  23.l] 
Similarly,  the  Fy  LQR  weightings  are  Q=50  and  R=.075  for  the  error  in  Fy  and 
the  ufy  control.  The  same  estimator  weightings  are  used  as  for  the  Fx  LQE  design, 
resulting  in  the  following  controller: 


-18.2  -11.6  -1.9  -5.2  -3.2  -11.3  -12.2  -15  -13.0  -11.5  -8.6  -19.1  103.3' 

-10  -173.4  -231  -72.0  -125.3  -137.4  -168.4  -146.2  -67.0  -53.6  -30.7  -14.9  0 

0  1024  0  0  0  -  6.2e-9  -5e-S  -5.1e-7  -  1.9e-6 -8.2£-6 -9.7^-6  -3.6^-5  0 

0  0  512  0  0  -5.5e-9-4.4e-8-4.5e-7-1.7^-6-7.2e-6-8.6f-6-3.2e-5  0 

0  0  0  256  0  3.0e-7  -2.4e-6  2Ae~5  9.0e-5  3,9e-4  4Je-4  lJe-3  0 

0  0  0  0  128  2.0e-7  1.6e-6  -\Je-5  6.2e-5  2.7^-4  3.2e~4  1.2c-3  0 

^=  0  0  0  0  0  128  -l.2e-5 -1.2e-4-4.4^-4-1.9£-3 -2.3e-3 -8.6e-3  0  (11*4) 

0  0  0  0  0  ~\Ae-6  128  -9^-5  -3.3e-4  -  1.5e-3  -  1.7e-3  -6.5e-3  0 

0  0  0  0  0  7.1e-6  -S.le-S  128  2.2^-3  9.4e-3  \Ae-2  4.2e-2  0 

0  0  0  0  0  2.8e-6  ~2.2e-5  2.3e-4  64  3.7^-3  4.4^-3  1.7e-2  0 

0  0  0  0  0  -8.7e-6  -7c-5  -7.2e-4 -2.7e-3  64  - 1.4^-2 -5.2e-2  0 

0  0  0  0  0  -2^-6  -1.6e-5-1.6e-4-6.U-4-2.7f-3  16  -1.2e-2  0 

000000  0  0  0  0  0  00 
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B  = 


A.6e~-3  0 
22e-6  0 
\.9e~5  0 
1.7^ -5  0 
-9.0^ -4  0 
-6.1e-4  0 
4.4e-3  0 
3.3e-3  0 
~22e-2  0 
-8.5e-3  0 
2.66-2  0 
6.1e-3  0 
-1  1 


.D 


[oo] 


c  =  [-2.1  -2.9  -0.49  -1.3  -0.80  -2.8  -3.1  -3.8  -3.2  -2.9  -2.2  -4.8  25.8] 

The  bandwidth  plots  for  the  LQG  Fx  and  Fy  controllers  are  shown  in  Figure  11.3 
and  Figure  11.4,  respectively.  The  bandwidth  for  the  Fx  controller  is  about  4  rad/s 
with  a  peak  of  about  70  rad/s  (near  the  1st  mode).  Importantly,  the  bandwidth  plot 
never  is  greater  than  0  dB’s,  so  no  frequencies  are  amplified. 

Similarly,  the  Fy  controller  bandwidth  is  also  about  4  rad/s.  The  first  4  modes  can 
be  seen  in  the  plot,  but  all  are  well  suppressed. 


Frequency  [rad/sec] 
LQG  fxm-fxd  BW 


Figure  11.3-  LQG  Fx  Controller  Bandwidth 
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LQQ  fym-tyd  BW 

Figure  11.4-  LQG  Fy  Controller  Bandwidth 

11.2.3  LQG  Reduced-Order  Force  Control 

The  LQG  controllers  designed  in  the  previous  section  are  both  13th  order  control¬ 
lers.  The  same  two  techniques  applied  to  reduce  the  LQG  position  controllers  are 
reapplied  here  to  these  LQG  controllers.  Namely,  balanced  truncation  and  modal  re- 
sidualization  techniques  are  used.  When  applied  to  the  LQG  Fx  controller,  both 
balanced  truncation  and  modal  residualization  produced  5th-order  controllers.  The 
balanced  truncation  controller  was  slightly  more  accurate  than  the  modal  residualiza¬ 
tion  in  matching  the  full-order  controller  by  examination  of  the  Bode  plots. 

The  resulting  5th-order  LQG  balanced  truncation  controller  is: 


-3.5  -63.4  6.0  2.0  8.1 

-3.1C-4  0 

63.4  -0.41  31.1  0.55  -2.7 

2.6c -4  0 

A  = 

6.0  -31.1  -10.6  -3.8  -13.1 

,B  = 

-  2.5c -3  0 

-2.0  0.55  3.8  -2.1  -2.1 

-6.4c -4  0 

0  0  0  0  0  _ 

-1  1_ 

C  : 

=  [-8.1  -2.7  13.1  2.1  23.3].^  =  [2.8c -6  0] 

For  the  LQG  Fy  controller,  both  reduction  techniques  produced  4th-order  con- 
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trollers.  Once  again,  examination  of  the  Bode  plot  showed  that  the  balanced 
truncation  reduced-order  controller  was  a  slightly  better  representation  of  the  full-or¬ 
der  controller.  Therefore,  the  balanced  truncation  controllers  were  used 
experimentally.  The  balanced  truncation  reduced-order  LQG  Fy  controller  is: 


-2.5c -2  -58.0  -0.61  -1.2 

1.4c -3  0 

A  = 

58.0  -3.7c -2 -1.56  1.4 

= 

2.2c -4  0 

-0.61  1.56  -19.7  -19.7 

-1.2c -3  0 

0  0  0  0  _ 

-1  1_ 

C  =  [1.2  1.4  19.7  28.l]>^  = 

-  2.0c  - 

-4  0] 

11.2.4  SANDY  Force  Control  Design 

The  SANDY  controller  is  designed  with  a  roll-off  filter  and  an  integral  state  for 
a  second  order  controller.  Essentially,  this  is  a  Proportional-Integral  (PI)  controller  in 
series  with  a  roll-off  filter.  There  are  two  plants  in  the  design  -  one  for  the  command 
input  and  one  for  the  noise  input  to  the  control.  By  allowing  a  plant  with  noise  on  the 
input,  a  penalty  is  placed  on  all  frequencies  and  allows  the  optimization  to  consider 
the  flexible  modes.  The  weightings  for  the  optimization  are  Ql=10  and  R1=0  for  the 
Fx  cormnand  error,  while  the  Fx  noise  error  has  weightings  of  Q2=l  and  R2=.2.  The 
optimization  starts  from  a  roll-off  of  15/(s+15)  and  C  matrix  gains  of  0.1  (corre¬ 
sponding  to  very  low  PI  gains).  The  A  matrix  is  not  optimized  to  keep  the  filter  and 
integral  state  eigenvalues  the  same,  and  the  D  matrix  is  not  optimized  to  preserve  the 
roll-off  filter  structure.  Also,  a  constraint  of  -0.2  is  placed  on  the  maximum  real  part 
of  the  closed-loop  eigenvalues.  The  resulting  SANDY  Fx  controller  is: 

The  bandwidth  plot  for  the  SANDY  Fx  controller  is  shown  in  Figure  1 1 .5.  Here, 
the  controller  is  shown  to  have  a  bandwidth  of  about  12  rad/s  with  a  peak  around  70 
rad/s,  as  with  the  LQG  Fx  controller. 

In  a  similar  manner,  the  SANDY  controller  is  again  optimized  around  a  roll-off 
filter,  this  time  at  25/(s+25).  This  design  involves  the  equivalent  two  plants  as  the  Fx 


0 

10.8 


.  C  =  [i2.5  3.2].  = 


0  0 
0  0 


(11.7) 
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design,  but  with  new  weightings  of  Ql=2.5,  R1=0,  Q2=l,  and  R2=.2.  Again,  the 
starting  point  for  the  optimization  is  the  fixed  roll-off  filter  and  C  matrix  gains  of  0. 1 . 


1  10  100  1000 
Frequency  [rad/sec] 

SANDY  2nd  Order  Fx,  fxm-fxd  BW 


Figure  11.5-  SANDY  Fx  Controller  Bandwidth 

Also,  the  same  maximum  closed-loop  eigenvalue  constraint  of  -.2  is  applied  to  the 
optimization,  and  the  same  variables  are  optimized  as  before.  The  resulting  SANDY 
Fy  controller  is: 


[0  -25 

The  corresponding  bandwidth  plot  for  the  SANDY  Fy  controller  is  shown  in  Fig¬ 
ure  1 1.6.  In  this  case,  the  bandwidth  is  around  1 1  rad/s,  and  the  first  four  modes  are 
sufficiently  attenuated. 


0 

12.2 


-C  =  [l4.3  4.o].£>  = 


0  0 
0  0| 


(11.8) 


11.3  Experimental  Force  Controller  Comparison 

The  test  profile  for  the  force  controllers  is  chosen  as  a  step  command  from  25  to 
50  gf  and  then  back  down  to  25  gf.  This  tests  the  controllers  for  applying  and  remov¬ 
ing  force  while  maintaining  contact.  An  LQG  position  controller  is  added  to  hold  the 
arm  at  the  same  position,  which  is  in  contact  with  a  straight  line  metal  surface.  For 
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both  the  Fx  and  Fy  tests,  0|  is  in  the  0°  position  and  02  is  in  the  90°  position.  The 
metal  surface  is  in  the  same  orientation  as  the  force  sine  sweep  tests. 


1  10  100  1000 
Frequency  [rad/sec] 

SANDY  2nd  Order  Fy,  fym-fyd  BW 

Figure  11.6-  SANDY  Fy  Controller  Bandwidth 

Figure  1 1 .7  shows  the  results  for  the  Fx  force  controller  tests.  It  is  clear  from  the 
plot  that  the  response  of  the  LQG,  reduced-order  LQG,  and  SANDY  Fx  controllers 
have  similar  responses.  The  PI  controller  has  a  slower  response  time  than  all  the 
others. 

Likewise,  the  Fy  force  controller  tests  are  shown  in  Figure  11.8.  Once  again,  the 
LQG,  reduced-order  LQG,  and  SANDY  controllers  all  have  comparable  perfor¬ 
mance.  Also,  the  PI  controller  performance  is  again  slower  than  the  others. 


Exp  PI  (.3,5),  LOG  (r=.075,q2s40),  LOG  5BT,  SANDY  Pl+r(15),  fx  command  (step) 

Figure  11.7-  Experimental  Controller  Fx  Comparison 


Exp  PI  (.7,6),  LOG  (r=.075,q2=50),LOG  4BT,SAN  R(25),  fy  command  (step) 


Figure  11.8-  Experimental  Controller  Fy  Comparison 


12  Hybrid  Control  Designs 

There  are  primarily  two  ways  to  design  for  a  hybrid  control  scheme.  The  first  is 
superposition,  where  the  force  controllers  and  the  position  controllers  are  designed 
independently  and  combined  as  a  hybrid  controller.  The  second  method  involves  a 
complete  hybrid  optimization,  where  all  controllers  are  designed  simultaneously. 
The  advantage  of  the  superposition  method  is  that  it  is  simpler,  and  previous  position 
and  force  controller  designs  can  be  used.  However,  it  involves  adjusting  three  sepa¬ 
rate  controllers,  and  can  be  very  time  consuming.  The  complete  optimization 
approach  is  more  complicated  since  it  involves  larger  plant  models  and  controllers, 
but  allows  one  to  simultaneously  affect  all  three  controllers.  Both  methods  can  pro¬ 
vide  good  results,  depending  on  the  amount  of  time  spent  on  the  design. 

12.1  Superposition 

The  first  cut  uses  the  robust  reduced-order  SANDY  position  and  reduced-order 
SANDY  force  controllers  described  in  previous  chapters.  However,  the  position  con¬ 
troller  is  not  bandwidth  separated  from  the  force  controllers,  and  has  a  tendency  to 
jerk  the  arm  off  of  the  surface  after  the  force  control  starts.  The  reason  for  the  sudden 
surge  in  position  control  is  that  the  force  control  initially  opposes  the  position  control 
while  allowing  a  force  buildup  on  the  surface.  Kraft’s  suggestion  of  bandwidth  sep¬ 
aration  is  used  [34],  and  requires  a  new  SANDY  position  controller  design. 

The  new  robust  reduced-order  controller  is  optimized  around  a  fixed  roll-off  filter 
at  15  rad/s  for  the  1st  input  (Gj  error)  and  another  roll-off  filter  at  16  rad/s  for  the  2nd 
input  (02  error).  The  new  weightings  were  Ql=5,  Q2=22.5,  and  Q3=0.5,0.5  for  each 
set  of  pseudo-plants  corresponding  to  the  6  plant  conditions  (18  total  pseudo  plants). 
Q1  is  the  weighting  on  the  Gj  input,  Q2  is  the  weighting  on  the  02  input,  and  Q3  is 
the  weighting  on  noise  inputs  to  both  Gj  and  02-  The  starting  point  of  the  optimization 
is  a  controller  with  similar  performance  to  the  original  SANDY  position  controller. 
The  resulting  SANDY  controller  is  6th  order,  with  2  integral  states,  2  roll-off  states, 
and  2  dynamic  states.  This  is  a  strongpoint  of  the  SANDY  program  -  it  allows  the 
designer  to  combine  classical  techniques,  such  as  roll-off  filters,  with  optimization 
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techniques.  The  resulting  SANDY  controller  has  the  form  found  in  Equation  (12.1) 
and  Equation  (12.2). 
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(12.2) 


The  filtered  bandwidth  plots  for  the  new  robust  6th  order  SANDY  controller  are 
shown  in  Figure  12.1  and  Figure  12.2.  The  bandwidth  for  the  shoulder  loop  is  about 
2.75  rad/s  while  the  bandwidth  for  the  elbow  loop  is  about  1.1  rad/s.  Once  again,  a 
command  prefilter  at  l/(s+l)  is  added  since  that  is  how  the  controller  was  optimized 
and  because  the  position  reference  is  fed  through  a  spline.  The  spline  acts  like  a  filter 
in  the  sense  that  the  reference  command  is  not  instantaneously  applied. 


Frequency  [rad/sec] 

SANDY  Robust  6th  Order  (pref=1),  th1-th1„cmd  BW 

Figure  12.1-  6th  Order  Robust  SANDY  Shoulder  Control  Bandwidth 

The  new  position  controller  is  combined  with  the  previous  SANDY  force  con- 
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trollers,  and  experimentally  evaluated  and  compared  to  the  full-order  LQG  position 
and  force  controllers.  The  overall  SANDY  hybrid  controller  is  10th  order,  and  the 
overall  LQG  hybrid  controller  is  46th  order. 


Frequency  [rad/sec] 

SANDY  Robust  6th  Order  (pref=1),  th2-th2_cmd  BW 


Figure  12.2-  6th  Order  Robust  SANDY  Elbow  Control  Bandwidth 

In  order  to  test  the  hybrid  control  scheme,  the  end-point  is  started  in  contact  with 
a  straight  smooth  metal  surface.  The  exact  trajectory  is  known  and  provides  the  po¬ 
sition  reference.  In  this  case,  a  0.4m  line  is  traversed  in  10  seconds  for  the  position 
reference.  Knowing  the  trajectory  is  not  uncommon  in  manufacturing  tasks  when  the 
shape  of  an  item  being  machined  is  known  but  the  user  wants  to  apply  a  constant 
force  (for  example,  to  polish  a  surface).  Thus,  the  task  is  to  follow  a  straight  line  sur¬ 
face  while  applying  a  50  gf  magnitude  surface  normal  force.  This  particular  line  is  a 
challenge  in  the  sense  that  it  requires  the  Fx  force  to  switch  from  positive  to  negative 
in  the  middle  of  the  run. 

To  compare  the  controllers,  first  the  full-order  LQG  position  controller  is  used 
with  the  two  LQG  force  controllers  previously  described.  Next,  the  reduced-order 
SANDY  position  controller  is  used  in  conjunction  with  the  three  sets  of  force  con¬ 
trollers  (LQG,  LQG  reduced-order,  and  SANDY).  All  the  experiments  are  presented 
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in  a  strip  chart  showing,  in  order,  the  commanded  voltage  to  the  actuators,  the  posi¬ 
tion  torques,  the  force  torques,  and  the  measured  forces.  Note  that  the  measured 
magnitude  is  the  resultant  force  of  the  measured  x  and  y  forces.  Additionally,  one  plot 
of  the  end-point  position  is  included  with  a  time  reference  (marked  by  an  ‘x’  at  0.5 
sec  intervals).  The  position  plots  are  all  similar  since  all  combinations  follow  the 
same  surface,  so  only  one  is  included.  The  units  for  the  voltage  conunands  are  Volts, 
for  the  torque  commands  are  Newton-meters  (Nm),  and  for  the  forces  are  grams- 
force  (gf);  When  interpreting  the  strip  charts,  realize  that  the  force  control  activation 
occurs  when  the  control  torques  for  the  forces  become  non-zero. 

Six  hybrid  control  combinations  are  tested  to  compare  reduced-order  controller 
performance  to  full-order  LQG  controller  performance  using  superposition  of  the 
force  and  position  controllers.  The  combinations  are  listed  in  Table  12.1. 


Table  12.1-  Hybrid  Superposition  Combinations 


Test 

Position 

Force 

Case 

Controller 

Controller 

1 

LQG  (Full) 

LQQ  (Full) 

2 

LQG  (Full) 

LQG  (Reduced) 

3 

LQG  (Full) 

SANDY  (2nd) 

4 

SANDY  (6th) 

LQG  (Full) 

5 

SANDY  (6th) 

LQG  (Reduced) 

6 

SANDY  (6th) 

SANDY  (2nd) 

The  first  test  case  matches  the  20th  order  LQG  position  controller  with  the  full- 
order  (13th)  LQG  Fx  and  Fy  force  controllers.  Figure  12.3  shows  the  end-point  posi¬ 
tion  during  the  test.  Notice  from  the  x  every  0.5  seconds  on  the  plot  that  the  end-point 
starts  out  slow,  speeds  up  during  the  middle  part  of  the  slew,  and  then  slows.  The 
“choppiness”  is  due  to  encoder  resolution  (about  +/-  2mm). 

The  corresponding  strip  chart  is  seen  in  Figure  12.4.  Notice  that  after  the  force 
control  is  activated  at  about  3.5  seconds,  it  takes  about  1.5  seconds  to  initially  reach 
the  desired  force  and  about  another  1.5  seconds  to  settle  after  a  10  gf  overshoot. 

Next,  the  reduced-order  LQG  force  controllers  (Fx  =  5th  order  and  Fy  =  4th  or¬ 
der)  are  tested  with  the  20th  order  LQG  position  controller  for  the  second  test  case. 


Figure  12.5  shows  that  there  is  a  slightly  degraded  performance  from  the  full-order 
controller. 


x_ep(m) 

xep-yep  with  50  gf  cmd  (x=.5sec)  LOG  For,  LQG  Pos,  .4m  in  10  sec 


Figure  12.3-  LQG  Position  and  LQG  Force  End-Point  History 
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LQG  force  (contact),  LQG  Pos,  .4m  line 

Figure  12.4-  LQG  Position  and  LQG  Force  Experimental  Data 
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It  takes  about  1.75  seconds  to  initially  reach  the  50  gf  point,  and  about  2  more  sec¬ 
onds  to  settle.  Also,  there  is  slightly  more  overshoot  for  the  reduced-order  LQG  force 
controllers. 


0  2  4  6  8  10  12  14  16  18  20 

t 

LQGR  force  (contact),  LQG  Pos,  .4m  line 

Figure  12.5-  LQG  Position  and  LQGR  Force  Experimental  Data 

The  third  test  case  matches  the  full-order  LQG  position  controller  with  the  re¬ 
duced-order  SANDY  force  controllers  (Fx  =2nd  order  and  Fy  =  2nd  order).  Figure 
12.6  shows  that  it  takes  about  1.25  seconds  to  build  up  to  the  required  50  gf  with  no 
overshoot.  This  is  actually  slightly  better  than  the  LQG  force  controller  combination! 

The  next  phase  of  experiments  uses  reduced-order  SANDY  position  controllers. 
The  first  force  controllers  used  with  the  SANDY  position  controller  are  the  LQG  full- 
order  (13th)  controllers.  Figure  12.7  shows  the  resulting  experimental  run  for  test 
case  4.  This  time,  after  initial  activation,  it  takes  roughly  1.5  seconds  to  reach  the  50 
gf  level,  and  then  roughly  1.5  additional  seconds  to  stabilize  after  an  undershoot.  The 
initial  undershoot  is  about  20  gf,  which  is  more  than  the  LQG  controller.  However, 
the  response  times  are  about  the  same. 

The  same  trends  continue  for  the  force  controllers  with  the  SANDY  position  con¬ 
troller  during  the  5th  test  case.  Figure  12.8  shows  that  the  reduced-order  LQG  force 
controllers  show  slight  degradation  from  full-order  LQG  force  controllers  in  the  form 


of  slightly  more  overshoot.  The  initial  response  time  is  about  2  seconds,  and  it  takes 
an  additional  3  seconds  to  stabilize.  The  overshoot  is  about  15  gf  in  this  case. 


0  2  4  6  8  10  12  14  16  18  20 


t 

SAN  force  (contact),  LQG  Pos,  .4m  line 

Figure  12.6-  LQG  Position  and  SANDY  Force  Experimental  Data 


LQG  force  (contact),  SAN  Pos,  .4ni  line 


Figure  12.7-  SANDY  Position  and  LQG  Force  Experimental  Data 
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0  2  4  6  8  10  12  14  16  18  20 
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LQGR  force  (contact),  SAN  Pos,  .4m  line 

Figure  12.8-  SANDY  Position  and  LQGR  Force  Experimental  Data 
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SAN  force  (contact),  SAN  Pos,  .4m  line 

Figure  12.9-  SANDY  Position  and  Force  Experimental  Data 

Finally,  Figure  12.9  shows  the  SANDY  position  and  force  experimental  data  for 
the  6th  test  case.  There  is  once  again  a  slight  improvement  (less  overshoot)  over  the 
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LQG  force  controllers.  However,  compared  to  the  LQG  position  control,  there  is  a 
little  more  overshoot  with  about  the  same  rise  time. 

Overall,  the  SANDY  reduced-order  force  controllers  provide  slightly  better  per¬ 
formance  than  the  full-order  LQG  force  controllers.  The  LQG  position  controller 
provides  slightly  better  performance  than  the  SANDY  reduced-order  position  con¬ 
troller.  Thus,  for  hybrid  control,  the  reduced-order  SANDY  controllers  provide 
comparable  performance  to  the  full-order  LQG  controllers. 

12.2  Complete  Optimization 

The  complete  optimization  can  involve  strictly  a  reduced-order  design  using  one 
plant,  or  can  involve  a  robust  reduced-order  design.  Both  approaches  are  covered  in 
this  section.  In  both  cases,  the  hybrid  controllers  have  the  structure  shown  in  Equa¬ 
tion  (12.3)  and  Equation  (12.4). 
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In  these  equations,  matrices  with  the  subscript  p  correspond  to  the  position  con¬ 
troller.  Also,  the  fx  and  fy  subscripts  correspond  to  the  Ex  and  Fy  controllers.  From 
this  structure,  the  position  inputs  are  fed  only  to  the  position  controller  while  the  fx 
and  fy  inputs  are  fed  only  to  their  respective  controllers.  This  allows  the  controllers 
to  perform  just  like  their  superposition  counterparts  while  allowing  simultaneous  or 
complete  optimization  within  SANDY. 

12.2.1  Complete  Reduced-Order  Optimization 

To  start  with,  a  reduced-order  hybrid  control  design  is  used.  This  involves  one  po¬ 
sition  model,  one  Fx  model,  and  one  Fy  model  combined  into  one  hybrid  position  and 
force  control  model.  The  same  pseudo-plants  used  for  the  previous  position  and  force 
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control  designs  are  used,  which  results  in  3  pseudo-plants  for  position,  2  for  Fx,  and 
2  for  Fy.  Thus,  one  hybrid  plant  requires  7  pseudo-plants  for  the  hybrid  optimization, 
as  shown  in  Table  12.2. 


Table  12.2  -  Hybrid  Pseudo-Plants 


Pseudo-Plant 

Purpose 

1 

Optimize  tor  error 

2 

Optimize  for  02  error 

3 

Optimize  for  high  freq 
(noise  into  both  6  control  inputs) 

4 

Optimize  for  Fx  errors 

5 

Noise  into  Fx  control  input 

6 

Optimize  for  Fy  errors 

7 

Noise  into  Fy  control  input 

The  actual  hybrid  plant  combines  the  position,  Fx,  and  Fy  plants  in  parallel,  so 
the  new  plant  is  46th  order  and  has  4  control  inputs,  4  reference  inputs,  and  6  outputs. 
The  control  and  reference  inputs  are  for  9j,  02,  fx,  and  fy.  The  outputs  are,  in  order, 

0j ,  02 , 6i_error,  62_error,  fx_error,  and  fy_error.  The  0  terms  are  used  only  in  the 

criterion,  thus  the  resulting  SANDY  hybrid  controller  has  4  inputs  (the  error  terms) 
and  4  outputs  (the  control  inputs  to  the  hybrid  plant).  This  hybrid  controller  is  actu¬ 
ally  a  position  controller,  an  Fx  controller,  and  an  Fy  controller  combined  in  parallel. 
Thus,  the  only  coupling  between  the  controllers  is  in  the  optimization  cost  function. 
Functionally,  the  complete  hybrid  SANDY  controller  has  the  same  structure  as  the 
superposition  method.  This  particular  design  uses  a  position  model  linearized  around 

the  01=0°  and  02=90®  nominal  conditions.  The  force  models  were  also  obtained  at  the 
same  angle  positions. 

The  optimization  weightings  are  shown  in  Table  12.3.  The  4  control  weightings 
apply  to  tip,  t2p,  Ufx,  and  Ufy.  The  actual  implementation  applies  the  Jacobian  to  the 
force  commands  to  generate  t^f  and  t2f.  The  position  and  torque  forces  are  added  to¬ 
gether  to  produce  the  actual  tj  and  t2  applied  to  the  plant.  In  addition,  the  maximum 
real  eigenvalue  is  constrained  to  -0.2  while  adding  a  minimum  damping  constraint  of 
0.7  for  all  frequencies  below  25  rad/s.  This  damping  constraint  allows  the  rigid  body 
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modes  to  have  a  minimum  damping  while  not  constraining  the  flexible  modes.  With¬ 
out  additional  sensors  and/or  actuators,  the  damping  on  these  flexible  modes  can’t  be 
effectively  controlled.  The  starting  point  for  the  optimization  is  the  previous  super¬ 
position  hybrid  controller.  In  this  way,  none  of  the  previous  optimization  work  is 
wasted. 


Table  12.3  -  Hybrid  Optimization  Weightings 


Pseudo-Plant 

Criterion  Penalty 

Control  Penalty 

. "T . . 

Ql=diag(U,0,5,U,U,U) 

Rl=diag(0,0,0,0) 

2 

Q2=diag(0, 0,0, 22.5, 0,0) 

R2=diag(0,0,0,0) 

3 

Q3=diag(0, 0,0.2, 0.2, 0,0) 

R3=diag(0,0,0,0) 

4 

Q4=diag(0, 0,0, 0,10,0) 

R4=diag(0,0,0,0) 

5 

Q5=diag(0,0,0, 0,1,0) 

R5=diag(0,0,.2,0) 

6 

Q6=diag(0,0,0,0,0,2.5) 

R6=diag(0,0,0,0) 

7 

Q7-diag(0,0,0,0,0,l) 

R7=diag(0,0,0,.2) 

These  weightings  produce  the  complete  hybrid  controller  shown  in  Equations 
(12.5)  through  (12.8). 
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(12.5) 


(12.6) 


=  [ll.6  3.3],^/,  =  [0]  (12.7) 

><^fy  =  [l5.6  2.6]»^/_y  =  [0]  (12.8) 

The  position  controller  has  the  same  inputs  and  outputs  as  the  superposition  po¬ 
sition  controllers.  Namely,  the  inputs  are  error_6i  and  error_02  while  the  outputs  are 
Tjp  and  T2p.  The  bandwidth  plot  of  the  shoulder  motor  in  Figure  12. 10  shows  that  the 
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bandwidth  is  about  1.75  rad/s.  The  elbow  motor  bandwidth  shown  in  Figure  12.11 
reveals  a  bandwidth  of  about  1.2  rad/s. 


SANDY  Comp  Hyb  6th  Order  (pref=1),  th1-th1_cmd  BW 

Figure  12.10-  Shoulder  Bandwidth,  SANDY  Hybrid  Position  Controller 


SANDY  Comp  Hyb  6th  Order  (prefssi),  th2-th2_cmd  BW 


Figure  12.11-  Elbow  Bandwidth,  SANDY  Hybrid  Position  Controller 
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The  force  bandwidth  for  the  Fx  controller  is  shown  in  Figure  12.12  as  11  rad/s 
while  the  Fy  bandwidth  shown  in  Figure  12.13  is  8  rad/s. 


hyb1d35  (Complete  Hybrid)  Fx,  fxm-fxd  BW 

Figure  12.12-  Fx  Control  Bandwidth,  SANDY  Hybrid  Controller 


1  10  100  1000 
Frequency  [rad/sec] 

hyb1d35  (Complete  Hybrid)  Fy,  fym-fyd  BW 


Figure  12.13-  Fy  Control  Bandwidth,  SANDY  Hybrid  Controller 
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The  resulting  complete  hybrid  controller  is  tested  along  the  same  trajectory  as  be¬ 
fore,  and  the  results  are  shown  in  Figure  12. 14.  It  takes  slightly  over  1  second  to  build 
up  to  50  gf  with  no  overshoot,  and  thus  gives  slightly  better  performance  than  the 
LQG  position  and  force  controller. 


0  2  4  6  8  10  12  14  16  18  20 

t 

hyb1  d35  {.7  min  -  25)  SANDY  force  (contact),  SANDY  Pos,  .4m  line 

Figure  12.14-  Complete  Hybrid  Optimization  Experimental  Data 

12.2.2  Complete  Robust  Reduced-Order  Optimization 

The  next  logical  step  is  to  add  multiple  plant  conditions  to  allow  a  robust  reduced- 
order  hybrid  design.  This  particular  effort  uses  2  position  models  linearized  at  differ¬ 
ent  angle  positions.  Logical  extensions  for  robustness  could  involve  variable  tip  mass 
and  even  more  linearized  angle  position  models.  Additionally,  multiple  force  models 
could  be  added.  The  cost  of  adding  more  plants  is  to  increase  the  required  computa¬ 
tion  time  in  the  optimization.  In  addition,  there  is  a  classic  trade-off  between 
robustness  and  performance.  Thus,  one  should  only  include  the  conditions  which  the 
plant  will  frequently  operate  in  to  avoid  unnecessary  performance  degradation  in  the 
primary  area  of  operation. 

The  first  position  plant  is  linearized  about  the  0i=2O®  and  02=110°  point,  while 
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the  second  position  plant  is  linearized  about  the  9i=-20°  and  02=70°  point.  The  key 

variation  here  is  the  02  angle,  which  varies  by  +/-  20°  from  the  plant  in  the  original 
design.  The  same  force  models  are  used  in  the  hybrid  plant,  which  combines  the  po¬ 
sition  model,  the  Fx  force  model,  and  the  Fy  force  model. 

The  same  optimization  constraints  are  applied  to  this  design  as  the  original  com¬ 
plete  hybrid  optimization,  namely  real  eigenvalue  and  frequency  dependent  damping 
constraints.  The  new  weightings  are  listed  in  Table  12.3.  Notice  that  the  penalties  had 
to  be  increased  on  the  position  controller  to  maintain  acceptable  performance. 


Table  12.4  -  Robust  Hybrid  Optimization  Weightings 


Pseudo-Plant 

Criterion  Penalty 

Control  Penalty  | 

1 

2 

Q2=diag(0,0,0,45,0,0) 

R2=diag(0, 0.01,0,0) 

3 

Q3=diag(0, 0,0.4, 0.4, 0,0) 

R3=diag(0.01, 0.01, 0,0) 

4 

Q4=diag(0, 0,0,0, 10,0) 

R4=diag(0,0,0,0) 

5 

Q5=diag(0, 0,0,0, 1,0) 

R5=diag(0,0, 0.05,0) 

6 

Q6=diag(0,0,0,0,0,2.5) 

R6=diag(0,0,0,0) 

7 

Q7=diag(0,0,0,0,0,l) 

R7=diag(0,0,0,0.05) 

These  weightings  produce  the  complete  robust  hybrid  controller  shown  in  Equa¬ 
tions  (12.9)  through  (12.12). 
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The  bandwidth  plots  for  robust  hybrid  position  shown  in  Figure  12.15  and  Figure 
12.16  show  a  shoulder  bandwidth  of  1.9  rad/s  and  an  elbow  bandwidth  of  1.25  rad/s. 


Frequency  [rad/sec] 

SANDY  Robust  Hyb  6th  Order  (pref=1),  th1-th1_cmd  BW 

Figure  12.15-  Shoulder  Bandwidth,  SANDY  Robust  Hybrid  Controller 


Figure  12.16-  Elbow  Bandwidth,  SANDY  Robust  Hybrid  Controller 
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The  Fx  bandwidth  plot  of  Figure  12. 17  shows  a  bandwidth  of  1 1  rad/s  while  the 
Fy  bandwidth  plot  of  Figure  12.18  reveals  a  bandwidth  of  8  rad/s. 
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Figure  12. 19  shows  the  experimental  response  of  the  robust  reduced-order  hybrid 
complete  optimization  controller.  In  this  case,  the  response  takes  slightly  over  1  sec¬ 
ond  to  ramp  up  to  50  gf  and  has  no  overshoot.  Once  again,  this  response  is  slightly 
better  than  the  full-order  hybrid  LQG  controller. 


0  2  4  6  8  10  12  14  1$  18  20 
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rp11a  (.7  min  -  25)  SANDY  force  (contact),  SANDY  Pos,  .4m  line 

Figure  12.19-  Complete  Robust  Hybrid  Optimization  Experimental  Data 

12.3  Hybrid  Design  Experimental  Conclusions 

Overall,  the  reduced-order  SANDY  controllers  perform  comparably  to  the  full- 
order  LQG  controllers.  In  addition,  performing  a  complete  hybrid  optimization  al¬ 
lowed  better  design  results  since  it  was  easier  to  adjust  all  the  controllers 
simultaneously.  It  is  possible  that  a  better  design  for  superposition  could  be  obtained 
by  iteratively  tuning  the  position,  Fx,  and  Fy  controllers.  However,  because  three 
controllers  are  involved,  it  is  more  difficult  than  the  complete  optimization  method 


13  Surface  Following  Experiments 

As  noted  earlier,  there  are  many  applications  where  a  robotic  manipulator  is  re¬ 
quired  to  follow  a  surface  and  apply  a  constant  force  to  perform  a  task.  These 
applications  include  buffing,  grinding,  polishing,  or  painting.  The  normal  approach 
to  tackle  these  problems  is  to  learn  the  desired  trajectory  and  then  perform  the  task. 
The  obvious  problem  here  is  that  every  new  shape  requires  learning  a  new  trajectory. 

The  particular  application  here  is  TLA.  The  sensors  used  are  a  wrist-mounted 
force  sensor  and  optical  encoders  at  the  two  hub  angles.  The  tool  mounted  to  the  wrist 
is  a  low-friction  roller  which  allows  measurement  of  almost  purely  surface  normal 
forces.  Using  the  surface  normal  and  hub  angle  information,  a  position  command  is 
given  along  the  surface  tangent.  Thus,  the  algorithm  allows  movement  along  an  un¬ 
known  surface  by  continually  commanding  motion  along  the  surface  tangent.  In 
order  to  see  how  effective  the  algorithm  is  for  the  TLA,  different  shapes  are  used.  Al¬ 
so,  the  effect  of  different  controllers  and  different  commanded  speeds  is  examined. 

All  the  tests  start  with  the  end-point  in  contact  with  the  surface,  and  then  a  slight 
position  offset  is  commanded  into  the  surface  for  2  seconds  to  allow  the  force  control 
to  engage.  Therefore,  if  the  algorithm  runs  for  5  seconds,  the  arm  actually  moves  for 
3  seconds.  This  allows  the  force  to  build  up  to  the  desired  level,  and  allows  for  a  com¬ 
mon  starting  state  for  all  the  experiments.  Two  plots  are  provided  for  each  test  -  a 
strip  chart  which  shows  the  measured  forces  and  commanded  torques,  and  an  x-y 
end-point  plot  which  shows  the  actual  end-point  location  (calculated  through  forward 
kinematics)  and  the  position  reference  provided  by  the  surface  following  algorithm. 

The  strip  chart  units  are  volts  for  the  physical  voltage  output  to  the  power  sup¬ 
plies,  Newton-meters  (Nm)  for  the  torques,  and  gram-force  (gf)  for  all  the  measured 
forces.  The  x-y  plot  has  units  of  meters,  and  the  dashed  line  is  the  reference  output. 
There  is  an  x  to  represent  data  at  0.5  second  intervals  on  the  x-y  plot  as  well. 

13.1  Surface  Shape  Effects 

Many  shapes  are  made  up  of  lines  and  arcs,  so  it  seems  logical  to  test  the  algo¬ 
rithm  on  these  shapes.  The  simplest  surface  is  a  straight  line,  and  the  first  experiment 
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commands  50  gf  with  a  velocity  of  1  cm/s  while  using  the  LQG  hybrid  controller  for 
5  seconds.  The  strip  chart  in  Figure  13.1  shows  that  the  force  is  maintained  at  50  gf 
with  a  slight  jump  as  the  algorithm  ends  due  to  the  sudden  slowing  of  the  arm. 


LQG  force,  LQG  Pos,  unknown  line,  1  cm  per  sec 

Figure  13.1-  LQG,  Line,  50gf  Command,  1  cm/s.  Strip 


The  x-y  plot  shown  in  Figure  13.2  shows  that  reference  for  a  line  is  indeed  a  line. 
Thus,  the  algorithm  successfully  tracks  a  straight  line  surface. 
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The  next  shape  tested  is  a  concave  arc,  which  is  another  conunon  surface  encoun¬ 
tered  in  manufacturing.  The  strip  chart  of  Figure  13.3  shows  that  the  force  level  is 
maintained  at  50  gf  during  tracking  while  having  only  a  slight  transition  while  the 
arm  is  stopped  after  7  seconds. 


LQG  for-pos,  unk  arc  50gf  1  cm  per  sec,  7  sec 

Figure  13.3-  LQG,  Concave  Arc,  50gf,  1  cm/s.  Strip 
Figure  13.4  shows  that  the  reference  and  actual  trajectory  are  both  clearly  a  con¬ 
cave  arc.  In  this  case,  the  algorithm  clearly  works  for  a  concave  arc. 


Figure  13.4-  LQG,  Concave  Arc,  50gf,  1  cm/s,  x-y 
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Another  common  surface  encountered  in  manufacturing  is  a  convex  arc.  In  this 
case,  the  time  for  the  test  is  6  seconds  while  maintaining  a  50  gf  contact.  The  strip 
chart  is  shown  in  Figure  13.5,  and  shows  that  the  force  is  maintained  for  the  tracking, 
but  that  a  large  force  overshoot  is  required  to  stop  the  arm  after  6  seconds.  This  is 
because  the  arm  tends  to  accelerate  over  the  convex  surface,  even  though  the  refer¬ 
ence  remains  relatively  smooth. 


i  lA. 
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LOG  for*pos,  unk  inv  arc  50gf  1  cm  per  sec,  6  sec 

Figure  13.5-  LQG,  Convex  Arc,  50  gf,  1  cm/s.  Strip 

The  corresponding  x-y  plot  in  Figure  13.6  shows  that  the  surface  following  algo¬ 
rithm  successfully  commands  a  convex  arc  trajectory.  Notice  the  large  overshoot  in 
position  due  to  the  rapid  acceleration  over  the  convex  arc. 

Overall,  the  algorithm  successfully  provides  a  surface  following  position  refer¬ 
ence  for  all  the  shapes.  The  speed  at  which  the  arm  moves  is  dependent  on  shape,  and 
does  not  provide  a  direct  correlation  to  the  speed  reference. 

13.2  Effect  of  Tracking  Speed  Reference 

The  major  way  a  user  may  affect  the  speed  at  which  the  algorithm  tracks  is  by 
varying  the  value  k  in  Chapter  10.  Since  the  sampling  time  is  constant,  varying  k 
changes  the  tracking  velocity  reference.  As  stated  earlier,  this  reference  velocity 
doesn’t  provide  a  direct  measure  of  the  velocity. 
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xep-yep  with  50  gf  cmd  (x=;.5sec)  LOG  For-Pos,  Unk  Inv  Arc  1  cm  per  sec,  6  sec 


Figure  13.6-  LQG,  Convex  Arc,  50  gf,  1  cm/s,  x-y 

Repeating  the  line  tracking  experiment  for  a  50  gf  command  and  a  2  cm/s  velocity 

reference  results  in  the  strip  chart  shown  in  Figure  13.7.  In  this  case,  the  algorithm  is 
only  run  for  4  seconds  due  to  the  increased  distance  traveled.  Notice  that  the  force 
tracking  has  degraded  slightly  from  the  1  cm/s  velocity  reference  experiment  for  the 
line  with  slightly  more  force  overshoot  at  the  beginning  and  end  of  the  trajectory. 
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LQG  for-pos,  unk  line  SOgf  2  cm-s,  4  sec 

Figure  13.7-  LQG,  Line,  SOgf  Command,  2  cm/s.  Strip 

The  corresponding  x-y  plot  is  shown  in  Figure  13.8.  The  algorithm  once  again 
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provides  a  good  line  reference  for  the  unknown  surface  it  follows.  Notice  that  the  dis¬ 
tance  traversed  is  further  in  less  time  than  the  1  cm/s  velocity  reference. 


x_ep(m) 

xep-yep  with  50  gf  cmd  (x=.5sec)  LOG  For-Pos,  Unk  Line  2  cm  per  sec,  4  sec 


Figure  13.8-  LQG,  Line,  50gf  Command,  2  cm/s,  x-y 
13.3  Effect  of  Commanded  Force 

All  of  the  previous  tests  are  conducted  with  a  commanded  contact  force  level  of 
50  gf.  To  ensure  the  algorithm  will  work  at  other  force  levels,  a  test  at  a  30  gf  is  per¬ 
formed. 
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LQG  for-pos,  unk  line  30gf  1  cm  per  sec,  5  sec 

Figure  13.9-  LQG,  Line,  30gf  Command,  1  cm/s.  Strip 
The  same  straight  line  surface  is  used  with  the  LQG  hybrid  controller  for  another 
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5  second  test  ran.  The  strip  chart  shown  in  Figure  13.9  reveals  an  initial  transient  in 
the  force  and  then  good  tracking  for  the  remaining  time. 

The  corresponding  x-y  plot  in  Figure  13.10  shows  after  the  initial  transient  that  a 
straight  line  is  well-represented  by  the  algorithm.  Also,  the  lower  force  allows  the 
arm  to  start  its  movement  more  quickly  due  to  the  lower  contact  force  with  the  wall. 
Once  the  movement  is  started,  the  tracking  speed  is  roughly  the  same  as  the  50gf 
experiment. 


Figure  13.10-  LQG,  Line,  30gf  Command,  1  cm/s,  x-y 
13.4  Effect  of  Different  Controllers 

To  verify  that  the  algorithm  isn’t  tuned  to  a  full-order  LQG  controller,  a  reduced- 
order  SANDY  controller  is  also  used.  The  SANDY  controller  has  a  higher  bandwidth 
position  controller,  so  it  provides  different  performance  than  the  LQG  controller.  As 
a  basis  of  comparison,  the  straight  line  surface  is  used  with  a  50  gf  command  for  3 
seconds.  The  resulting  strip  chart  in  Figure  13.11  shows  that  the  force  is  maintained 
fairly  well  during  tracking,  with  an  undershoot  in  force  at  the  end.  It  is  interesting  that 
this  controller  has  an  undershoot  in  force  at  the  end  of  movement  but  the  LQG  con¬ 
troller  has  an  overshoot. 

Figure  13.12  clearly  shows  that  a  line  reference  is  provided,  even  with  a  different 
controller.  Notice  that  there  is  a  faster  speed  for  this  higher  bandwidth  controller. 
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which  causes  more  overshoot  and  slightly  degraded  performance. 
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SAN  for-pos,  unk  line  SOgf  1  cm  per  sec,  3  sec 

Figure  13.11-  SANDY,  Line,  SOgf  Command,  1  cm/s.  Strip 


x_op(m) 

xep-yep  with  50  gf  cmd  (x=.5sec)  SAN  For-Pos,  Unk  Line  1  cm  per  sec,  3  sec 


Figure  13.12-  SANDY,  Line,  SOgf  Command,  1  cm/s,  x-y 

13.S  Modified  Tangent  Speed  Command 

One  thing  the  experiments  show  is  that  the  speed  at  which  the  algorithm  tracks 
varies  with  shapes,  commanded  force,  and  the  bandwidth  of  the  position  controller. 
In  addition,  the  arm  speed  seemed  to  increase  as  the  algorithm  progressed  due  to  the 
projection  of  the  last  command  on  the  tangent  line  becoming  larger. 
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The  modified  algorithm  commands  a  constant  offset  along  the  tangent  line,  in¬ 
stead  of  an  offset  plus  a  previous  command  projection.  The  expected  effect  is  to 
provide  a  more  uniform  rate  of  tracking,  although  the  startup  time  will  be  longer  since 
the  reference  error  will  not  increase.  Using  the  illustration  of  the  algorithm  in  Chapter 
10,  essentially  the  offset  is  commanded  from  point  P2  and  not  P3. 

First,  the  algorithm  is  applied  to  the  same  straight  line  as  before  with  50  gf  ap¬ 
plied  for  5  seconds.  Figure  13.13  shows  in  the  resulting  strip  chart  that  the  force  is 
maintained  at  the  50  gf  level  during  the  tracking  and  at  the  end.  Since  the  arm  is  mov¬ 
ing  more  slowly,  there  is  no  force  overshoot  at  the  end. 
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LOG  for-pos,  unk  line,  mod,  50gf  1  cm  cmd,  5  sec 

Figure  13.13-  LQG,  Line,  50gf,  Mod,  1  cm  cmd,  strip 

The  corresponding  x-y  plot  is  shown  in  Figure  13.14.  In  this  case,  the  distance 

traveled  is  slightly  less  than  the  original  algorithm  and  the  acceleration  a  little 
smoother.  Clearly,  the  tracking  aspect  is  unaffected  by  the  change  in  speed  in  this 
case  as  the  line  reference  is  given  for  the  unknown  line  surface. 

Applying  the  modified  algorithm  to  a  concave  arc  for  15  seconds  with  50  gf  ap¬ 
plied  results  in  the  strip  chart  shown  in  Figure  13.15.  The  resulting  force  profile  is 
smooth  with  just  a  small  force  disturbance  at  the  end  of  the  run. 
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Kep'-yep  with  50  gf  cmd  (x^^.Ssec)  LOG  For-Pos,  Link  Line,  mod,  1  cm  cmd,  5  sec 

Figure  13.14-  LQG,  Line,  50gf,  Mod,  1  cm  cmd,  x-y 


LQG  for-pos,  unk  arc,  mod,  50gf  1  cm  cmd,  15  sec 

Figure  13.15-  LQG,  Concave  Arc,  50gf,  Mod,  1  cm  cmd,  strip 

The  x-y  plot  in  Figure  13. 16  shows  a  uniform  spacing  of  the  distance  traveled  per 

0.5  second  time  hash.  Thus,  the  modified  algorithm  regulates  the  speed  better  for  this 
case.  Notice,  however,  that  it  takes  a  lot  longer  for  the  algorithm  to  start  since  the  po- 


sition  reference  signal  is  constant. 
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x_ep(m) 

xep-yep  with  50  gf  cmd  (x=.5sec)  LQG  For-Pos,  Unk  Arc, mod,  1cm  cmd,  iSsec 

Figure  13.16-  LQG,  Concave  Arc,  50gf,  Mod,  1  cm  cmd,  x-y 

SAN  for>pos,  unk  line,  mod,  50gf  1  cm  cmd,  5  sec 


Figure  13.17-  SANDY,  Line,  50gf,  Mod,  1  cm  cmd,  strip 

Examining  the  effect  of  a  different  controller  on  the  modified  algorithm  provides 
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a  good  test.  The  SANDY  controller  is  once  again  used  as  the  basis  of  comparison  for 
the  straight  line  trajectory  for  5  seconds  while  applying  a  50  gf  force.  The  strip  chart 
shown  in  Figure  13.17  reveals  that  the  force  is  well-maintained  until  the  end  of  the 
run,  when  a  force  undershoot  occurs. 

Figure  13.18  shows  that  the  distance  traveled  by  the  arm  is  further  for  the 
SANDY  controller,  and  that  there  is  still  some  acceleration  as  the  run  progresses.  The 
higher  speed  causes  a  position  overshoot  and  the  force  discrepancy  at  the  end  of  the 
5  second  algorithm  application.  This  shows  that  the  algorithm’s  speed  is  still  depen¬ 
dent  on  the  position  controller. 
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x_ep(m) 

xep-yep  with  50  gf  cmd  (x-.Ssec)  SAN  For-Pos,  Unk  Line,  mod,  1 0  cm  cmd,  5  sec 

Figure  13.18-  SANDY,  Line,  50gf,  Mod,  1  cm  cmd,  x-y 
13.6  Surface  Following  Conclusions 

The  surface  following  algorithm  works  for  all  the  shapes  tested.  In  addition,  dif¬ 
ferent  force  levels,  different  commanded  speeds,  and  different  controllers  all  work 
successfully  with  the  algorithm.  However,  the  speed  at  which  the  algorithm  tracks 
varies  with  shape,  force  level,  and  controller. 

To  remedy  the  speed  variation  concern,  a  modified  surface  following  algorithm 
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which  commands  a  constant  distance  along  the  surface  tangent  was  examined.  The 
modified  algorithm  did  a  better  job  of  providing  constant  speed,  but  there  was  still 
some  acceleration  once  the  arm  was  in  motion.  The  speed  increase  was  especially  no¬ 
ticeable  with  the  higher  bandwidth  SANDY  controller.  The  disadvantage  of  the 
modified  algorithm  is  that  it  takes  longer  to  start,  and  it  doesn’t  always  overcome  the 
initial  stiction  of  the  roller  force  contact  with  the  surface. 


14  Surface  Touchdown 


One  of  the  more  challenging  aspects  of  robotics  is  performing  tasks  while  in  con¬ 
tact  with  a  surface.  In  such  a  contact  case,  not  only  the  position  but  the  force  must  be 
controlled.  This  is  called  a  constrained  motion  problem  if  the  surface  is  rigid,  or  a 
“compliant  motion  task”  for  a  compliant  surface  [52].  However,  the  transition  from 
free-space  motion  to  post-contact  force  control  can  be  even  more  challenging.  This 
surface  touchdown  problem  must  address  the  tendency  of  the  robot  to  “bounce”  after 
hitting  the  surface.  Once  the  surface  contact  is  lost,  the  force  control  is  no  longer  val¬ 
id.  Thus,  a  successful  touchdown  will  avoid  any  bounce.  This  touchdown  phase  of 
the  robotic  task  is  sometimes  called  impact  control  [47]. 

Kraft’s  baseline  hybrid  control  scheme  is  described  in  Chapter  10.  When  used  for 
touchdown,  the  block  diagram  must  be  modified  to  allow  pure  position  control  when 
not  in  contact  with  a  surface.  Thus,  a  block  is  added  which  checks  the  force  level,  as 
shown  in  Figure  14. 1 .  When  the  measured  force  is  less  than  the  defined  threshold,  the 
force  control  output  is  set  to  zero. 


Torque 


Force 


Figure  14.1-  Baseline  Hybrid  Force  and  Position  Control  Scheme 

For  this  research,  the  thresholds  are  experimentally  tuned  to  7  gf  for  initial  con¬ 
tact  and  4  gf  after  that.  The  reason  0  gf  isn’t  chosen  is  due  to  the  fact  that  the  force 
sensor  is  an  electrical  device,  and  has  an  electrical  zero  which  varies  slightly  through¬ 
out  the  run.  In  addition,  the  movement  of  the  arm  generates  a  small  amount  of 
dynamic  force.  Once  contact  is  made,  a  lower  threshold  is  allowed  to  allow  a  greater 
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chance  of  maintaining  contact,  especially  after  the  initial  impact. 

The  impact  can  be  described  by  basic  dynamics  equations  to  give  some  a  priori 
insight.  Assuming  perfectly  elastic  collisions,  the  following  equation  [50]  describes 
the  impact  of  two  rigid  bodies. 

mjMj+m2M2  =  (14.1) 

where 

uj,  U2  =  speeds  of  bodies  1  and  2,  respectively,  before  impact 

V],  V2  =  speeds  of  bodies  1  and  2,  respectively,  after  impact 
mj,  m2  =  massed  of  bodies  1  and  2,  respectively 

In  this  case  of  one  of  the  bodies  being  fixed,  like  in  an  impact  control  application, 
the  velocity  of  the  fixed  object  is  always  zero.  Thus,  the  equation  simplifies  to 

OTjMj  =  m^v^  (14.2) 

In  words,  the  impact  is  dependent  upon  both  the  speed  and  the  mass  of  the  manipu¬ 
lator.  The  resulting  velocity,  of  course,  is  in  the  opposite  direction.  A  higher  momen¬ 
tum  of  the  manipulator  as  it  impacts  with  the  surface  results  in  a  greater  tendency  to 
lost  contact.  Thus,  the  dynamics  say  that  larger  masses  and/or  higher  speeds  will  re¬ 
sult  in  a  more  difficult  touchdown. 

14.1  Baseline  Touchdown  Results 

Analyzing  the  baseline  system  gives  insight  into  possible  touchdown  solutions. 
All  the  tests  in  this  chapter  are  run  at  a  1  ms  sampling  time.  The  manipulator  is  run 
at  various  speeds  into  a  rigid  metal  surface,  without  knowledge  of  the  exact  surface 
location.  This  is  accomplished  by  commanding  a  position  which  would  take  the  ma¬ 
nipulator  somewhere  into  the  surface.  It  is  assumed  that  the  general  location  of  the 
surface  is  known. 

The  speed  tests  show  that  impact  force  is  directly  proportional  to  impact  speed, 
as  expected.  First,  no  force  control  is  applied  to  observe  the  resulting  impact  force 
with  just  position  control.  Then,  the  same  speeds  are  repeated  and  force  control  is  ap¬ 
plied  upon  impact  (defined  as  the  instant  7  gf  is  first  achieved).  Figure  14.2  shows 
that  when  force  control  is  applied,  the  impact  force  is  slightly  higher  than  with  pure 


153 


position  control.  This  is  likely  due  to  the  force  control  helping  push  the  arm  further 
into  the  surface  initially  while  trying  to  achieve  the  commanded  force  level. 

Next,  with  the  hybrid  control  applied,  the  speed  at  which  successful  touchdown 
(maintaining  surface  contact)  occurs  is  examined.  For  all  of  these  speed  tests,  a  50gf 
reference  level  is  used  (since  this  is  the  level  used  in  previous  research  on  the  arm). 
At  a  speed  of  0.6  cm/s,  the  touchdown  is  always  smooth  with  no  discernible  bounce. 
The  plot  of  this  experiment  is  shown  in  Figure  14.3.  Notice  that  the  touchdown  recoil 
is  low  after  an  initial  contact,  and  that  the  impact  force  is  about  20  gf.  Experiments 
showed  that  the  higher  the  impact  force,  the  greater  the  recoil,  as  expected. 
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Impact  Speed  (cm  per  s) 

Impact  Speed  vs.  Force  •  Hybrid  (o)  and  Pos  (x) 

Figure  14.2-  Impact  Force  vs.  Impact  Speed 

At  0.8  cm/s,  the  force  level  is  maintained  about  60%  of  the  time,  and  there  is  a 
noticeable  impact  recoil.  When  the  speed  is  increased  to  1.6  cm/s,  contact  is  never 
maintained  and  the  naked  eye  can  discern  a  loss  of  contact  with  the  surface  for  1 
bounce.  As  the  impact  speed  is  increased  further,  several  bounces  are  required  before 
contact  is  maintained.  The  plot  for  the  1.6  cm/s  experiment  is  shown  in  Figure  14.4. 
This  plot  gives  insight  into  the  baseline  limit  for  maintaining  contact.  The  units  on  all 
the  experimental  plots  are  volts.  Newton-meters  (for  the  torques),  and  gf  for  the  forc¬ 
es.  Examination  of  this  figure  shows  that  the  initial  impact  builds  up  to  just  past  50 


gf  in  the  first  0. 1  second,  and  rapidly  drops  off  for  about  the  next  0.2  seconds  causing 
the  arm  to  lose  contact  with  the  surface.  This  is  due  to  the  impact  with  the  surface 
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Baseline  Hybrid  Control,  0.6  cm  per  sec,  50  gf  ref 

Figure  14.3-  0.6  cm/s  Touchdown  with  50  gf  Reference,  Baseline  Performance 
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Baseline  Hybrid  Control,  1 .6  cm  per  sec,  50  gf  ref 

Figure  14.4- 1.6  cm/s  Touchdown  with  50  gf  Reference,  Baseline  Performance 

The  system  bandwidth  is  not  high  enough  to  respond  to  this  impact,  even  though  once 
in  contact  these  controllers  give  excellent  results.  Thus,  it  appears  that  some  kind  of 
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“bandwidth  boost”  is  needed  for  about  the  first  0.3  -  0.5  seconds  after  impact. 

Next,  several  other  factors  which  affect  the  touchdown  performance  are  exam¬ 
ined.  Specifically,  the  commanded  force  level  and  position  reference  are  analyzed. 
Force  reference  commands  of  20  gf,  50  gf,  and  150  gf  are  used  at  a  0.8  cm/s  speed. 
For  the  20  gf  level,  there  was  degraded  performance  and  contact  was  never  main¬ 
tained.  At  the  150  gf  level,  contact  was  maintained  on  every  attempt.  The  50  gf  was 
used  in  the  original  baseline  test  discussed  earlier.  Thus,  increasing  the  reference 
force  conunand  enabled  a  better  touchdown  performance. 

When  the  position  reference  was  changed  from  somewhere  in  the  surface  to  the 
exact  location  measured  at  touchdown,  there  was  a  noticeable  performance  degrada¬ 
tion.  Examine  the  baseline  case  shown  in  Figure  14.5,  where  at  0.8  cm/s  contact  is 
maintained  sometimes.  Even  when  the  contact  is  lost,  it  is  only  for  a  short  time. 
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Baseline  Hybrid  Control,  0.8  cm  per  sec,  50  gf  ref 


Figure  14.5-  0.8  cm/s  Touchdown  with  50  gf  Reference,  Baseline  Performance 

Contrast  this  with  Figure  14.6,  where  the  position  reference  is  changed  from  into 
the  surface  to  the  exact  touchdown  location  upon  impact.  Notice  that  the  recoil  is  am¬ 
plified  because  now  the  position  torques  oppose  the  force  torques.  Thus,  providing  a 
reference  slightly  into  the  surface  helps  the  surface  touchdown.  This  is  due  to  the  fact 
that  with  a  position  reference  into  the  surface,  the  position  torques  are  in  the  same 
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direction  as  the  force  torques.  The  overall  baseline  test  observations  are  summarized 
in  Table  14.1. 
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Figure  14.6-  0.8  cm/s  Touchdown  with  50  gf  Reference,  Surface  Reference 


Table  14.1-  Factors  Affecting  Touchdown 


1  Factor 

Effect  1 

Increased  Force  Reference 

Increased  Performance 

Position  Reference  Into  Surface 

Increased  Performance 

14.2  Touchdown  Control  Strategies 

By  analyzing  the  baseline  results,  improvement  areas  stand  out  for  both  the  posi¬ 
tion  and  force  schemes.  For  the  position  reference,  provide  a  location  into  the  surface 
so  that  the  position  torques  are  in  line  with  the  force  torques.  Maintain  this  offset  for 
at  least  1  second  after  impact  until  firm  contact  has  been  established.  Experience  has 
shown  that  a  moderate  offset  of  about  1  inch  is  adequate  for  this  application. 

The  force  aspect  of  the  hybrid  scheme  can  be  improved  several  ways.  First,  pro¬ 
viding  a  temporary  “bandwidth  boost”  immediately  after  impact  when  it  is  most 
needed  seems  logical.  Also,  providing  a  temporary  “force  boost”  to  the  force  refer- 
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ence  command  holds  promise.  The  modified  hybrid  control  structure  to  include  these 
modifications  are  shown  in  Figure  14.7. 


Torque 


Figure  14.7-  Modified  Hybrid  Force  and  Position  Control  Scheme 

A  simple  design  procedure  is  discussed  to  improve  surface  touchdown  tasks. 
There  are  four  degrees  of  freedom  for  the  surface  touchdown  task: 

1.  Vary  length  of  time  that  the  force  boost  is  applied  after  touchdown. 

2.  Vary  multiplication  factor  for  the  force  boost. 

3.  Vary  length  of  time  that  the  bandwidth  boost  is  applied  after  touchdown. 

4.  Vary  multiplication  factor  for  the  bandwidth  boost. 

These  four  factors  are  varied  iteratively  until  an  acceptable  touchdown  response 
is  achieved.  The  gain  factors  and  length  of  application  for  both  parts  of  the  scheme 
don’t  have  to  be  the  same,  although  they  may  be  identical  if  the  user  desires.  Notice 
that  the  force  and  position  controllers  don’t  have  to  be  redesigned,  which  is  an  advan¬ 
tage  to  this  approach. 

14.3  Experimental  Implementation 

Using  this  scheme,  a  force  boost  of  2  is  applied  for  0.3  seconds  after  impact  and 
a  bandwidth  boost  of  2  is  applied  for  0.5  seconds.  This  is  applied  to  the  nominal  test 
case  of  1.6  cm/s  impact  speed  and  a  50  gf  comihanded  force  level,  and  the  results  are 
shown  in  Figure  14.8.  The  same  force  and  position  controllers  are  used,  and  only  the 
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touchdown  modifications  are  different  between  this  test  and  the  baseline.  Notice  that 
the  force  level  never  drops  below  20  gf,  and  that  contact  is  maintained.  Compare  this 
to  the  nominal  case  of  Figure  14.4,  where  contact  is  never  maintained  at  this  speed 
and  commanded  force  level.  Thus,  this  method  provides  a  noticeable  improvement 
over  the  nominal  case. 


Mod  Hyb  Control,  1.6  cm  p«r  s«c,  SO  gf  ref,  force  boost  x2,.3,  BW  boost  x2,.5 

Figure  14.8-  Experimental  Results,  Modified  Touchdown  Scheme 


15  The  Complete  Hybrid  Task 


In  a  typical  manufacturing  task,  there  are  several  stages.  The  tool  first  starts  in 
some  arbitrary  location  and  then  it  makes  contact  with  a  work  surface.  Once  in  con¬ 
tact,  hybrid  control  is  applied  to  control  both  position  and  force  of  the  tool.  Finally, 
the  tool  must  be  removed  from  the  surface  and  returned  to  the  initial  arbitrary  loca¬ 
tion.  This  is  known  as  the  complete  hybrid  task  in  this  work. 

An  analytical  approach  to  the  complete  hybrid  task  divides  it  into  6  distinct  phas¬ 
es.  Figure  15.1  shows  the  phases  and  where  they  occur  during  the  complete  hybrid 
task. 
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Figure  15.1-  Complete  Hybrid  Task  Description 

During  Phase  1,  the  tool  is  moved  from  the  initial  arbitrary  location  towards  the 
surface.  The  position  reference  must  be  in  the  general  direction  of  the  surface,  and 
must  be  somewhere  into  the  surface.  Otherwise,  the  tool  would  never  make  contact 
with  the  surface.  During  this  phase,  only  the  position  control  is  applied. 

Phase  2  starts  when  the  tool  makes  contact  with  the  surface,  defined  as  the  first 
instant  that  7  gf  is  measured  by  the  force  sensor.  This  value  is  chosen  since  the  force 
sensor  is  an  electrical  device  with  an  electrical  zero  which  may  vary  slightly.  Also, 
when  the  arm  is  in  motion,  there  are  some  dynamic  forces  present.  7  gf  is  high  enough 
to  distinguish  between  dynamic  forces  and  electrical  zeros  versus  an  actual  impact 
for  this  application.  The  specifics  of  the  surface  touchdown  or  impact  control  are  de- 
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scribed  in  a  previous  chapter.  This  phase  lasts  for  1  second,  which  allows  enough 
time  to  maintain  good  contact  between  the  surface  and  the  tool. 

After  Phase  2  ends.  Phase  3  allows  transition  from  the  current  reference  position 
to  the  position  where  the  manipulator  first  made  contact  with  the  surface.  This  allows 
the  complete  hybrid  task  to  work  when  the  exact  position  of  the  surface  is  not  known. 
When  the  impact  is  initially  detected  (at  7  gf),  the  end-point  position  is  saved.  Then, 
at  the  start  of  Phase  3,  the  position  reference  is  changed  to  allow  a  smoother  transition 
for  surface  tasks. 

Some  experimental  observations  for  Phase  3  are  presented  now  to  give  some 
physical  insight  into  why  this  phase  is  required.  If  the  initial  reference  is  very  far  from 
the  surface,  a  large  step  command  is  essentially  generated  which  the  manipulator 
can’t  respond  to  instantaneously.  Thus,  to  ease  the  transition,  the  reference  at  the  sur¬ 
face  is  applied  and  the  manipulator  is  given  1  second  to  stabilize  before  starting  the 
constrained  hybrid  control  task.  There  is  a  tradeoff  between  how  far  the  initial  posi¬ 
tion  reference  command  is  given  into  the  surface  and  the  Phase  3  performance.  The 
further  inward  the  initial  reference  is  given,  the  better  the  impact  stage  response  since 
the  position  control  aids  the  force  control.  However,  a  greater  difference  between  the 
reference  and  the  wall  results  in  a  more  difficult  transition  to  the  constrained  hybrid 
control  stage  due  to  a  larger  step  change  in  the  position  reference. 

Additionally,  link  2  has  the  ability  to  move  and  follow  its  reference,  while  link  1 
is  physically  constrained  by  the  wall  (through  link  2  and  the  roller  mechanism).  Thus, 
upon  impact  with  a  reference  far  into  the  surface,  the  tool  has  a  tendency  to  roll  to  the 
right.  The  further  the  reference  is  into  the  surface,  the  further  to  the  right  the  tool 
moves.  Of  course,  since  Phase  2  lasts  only  1  second,  the  full  effect  is  not  seen.  For 
this  application,  1  second  for  Phase  3  works  well,  but  the  length  of  time  could  be  var¬ 
ied  for  another  system. 

Once  the  tool  is  in  contact  with  the  surface  and  is  at  a  known  reference  location. 
Phase  4  begins.  This  is  the  hybrid  position  and  force  scheme  described  in  earlier 
chapters.  The  surface  position  may  be  either  known  or  unknown,  depending  on  the 
application.  The  unknown  surface  following  algorithm  described  in  previous  chap- 
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ters  allows  an  unknown  constrained  surface  to  be  followed.  Of  course,  in  many 
manufacturing  applications,  the  exact  surface  shape  is  known,  and  can  be  used  during 
this  phase.  This  is  the  most  important  phase  of  the  complete  hybrid  task  in  the  sense 
that  the  actual  work  is  accomplished  during  this  phase. 

When  the  constrained  hybrid  task  is  completed.  Phase  5  of  the  complete  hybrid 
task  can  begin.  After  a  specified  time,  the  tool  must  be  withdrawn  from  the  surface. 
Experimental  analysis  showed  that  an  instantaneous  cutout  of  the  force  control  re¬ 
sults  in  an  undesirable  transient  response  of  the  manipulator.  This  transient 
manifested  itself  in  a  discernible  vibration  which  could  be  seen  with  the  naked  eye. 
Thus,  Phase  5  uses  the  same  spline  function  used  for  the  position  reference  to  spline 
both  torques  associated  with  the  force  controllers  from  their  values  at  the  start  of 
Phase  5  down  to  zero.  The  time  of  this  spline  is  a  design  variable,  and  this  application 
used  a  value  of  0.5  seconds. 

Finally,  after  the  force  torques  are  splined  down  to  zero.  Phase  6  begins.  The  cur¬ 
rent  end-point  position  is  saved,  and  the  initial  starting  position  is  known.  This  allows 
a  position  reference  spline  from  the  current  location  to  the  initial  location.  This  phase 
is  purely  position  control,  just  like  the  Phase  1 .  The  time  taken  for  this  phase  depends 
on  the  location  of  the  end-point  relative  the  starting  location.  If  the  two  points  are  far 
apart,  more  time  may  be  required  for  this  last  phase.  For  this  application,  5  seconds 
worked  well  for  all  of  the  experiments  performed. 

This  complete  task  should  work  for  different  controllers  and  for  arbitrary  shapes. 
The  two  controllers  used  for  the  testing  are  the  full  46th-order  LQG  hybrid  position 
and  force  controller  and  the  10th  order  robust  reduced-order  SANDY  controller.  All 
of  the  strip  charts  have  units  of  volts  (for  voltages),  Nm  (for  torques),  and  gf  (for 
forces). 

The  LQG  controller  is  applied  to  a  straight  line  without  knowledge  of  the  line’s 
shape  or  exact  location,  and  the  results  are  shown  in  Figure  15.2.  The  commanded 
speed  for  the  surface  algorithm  is  1  cm/s  and  the  commanded  force  is  50  gf  for  the 
constrained  motion  hybrid  task.  Phase  1  lasts  approximately  2.5  seconds,  until  con¬ 
tact  with  the  wall  occurs.  Notice  that  there  is  an  initial  recoil  in  the  force  level  due  to 
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the  collision  with  the  wall  at  about  2.5  seconds,  corresponding  to  the  start  of  Phase  2. 
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Retract,  LOG,  .005  offset,  1  s  hold,  1  s  wall,  unk  line,  1  cm  per  sec 


Figure  15.2-  LQG,  Unknown  Line  Strip  Chart 

Phase  3  begins  at  about  3.5  second,  causing  a  dip  in  the  force  level  due  to  the  transi¬ 
tion  of  the  position  reference  to  the  wall  position.  At  about  4.5  seconds,  the  unknown 


163 


surface  following  algorithm  starts,  and  there  is  a  small  increase  in  the  measured  force 
corresponding  to  the  start  of  Phase  4.  Notice  that  during  Phase  4,  the  force  level  is 
maintained  at  the  commanded  50  gf  level  while  tracking  the  unknown  surface.  After 
7  seconds,  Phase  5  begins  and  notice  that  the  force  torques  are  gradually  splined 
down  to  0  Nm  after  an  additional  0.5  second.  Finally,  Phase  6  involves  the  position 
slew  back  to  the  initial  position.  Notice  that  the  force  level  drops  to  zero  after  the  tool 
leaves  the  surface. 

The  corresponding  end-point  location  is  shown  in  Figure  15.3.  The  tool  starts 
about  0.025  meters  from  the  surface  (about  1”),  and  the  reference  is  a  line  in  the  gen¬ 
eral  direction  of  the  surface.  The  dashed  line  is  the  reference  signal  and  the  solid  line 
corresponds  to  the  end-point  location.  The  x’s  correspond  to  0.5  second  intervals,  and 
allow  the  end-point  position  to  be  related  back  to  the  strip  chart. 

Notice  that  the  reference  signal  changes  from  somewhere  into  the  surface  to  the 
point  of  initial  contact.  When  the  unknown  surface  algorithm  is  engaged,  observe  that 
a  relatively  straight  line  reference  is  given.  After  Phase  5  is  completed,  notice  that 
reference  changes  to  the  current  location  and  the  reference  is  a  straight  line  back  to 
the  initial  starting  point 


x_ep(m) 

xep-yep  with  50  gf  cmd  (x^^.Ssec)  LOG,  Unk  Line  Task,  1  cm  per  sec 


Figure  15.3-  LQG,  Unknown  Line  End-Point  Position 
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The  next  experiment  tests  the  reduced-order  SANDY  controller  at  a  rate  of  0.7 
cm/s  and  50  gf  commanded  for  the  same  straight  line  surface.  The  corresponding 
strip  chart  is  shown  in  Figure  15.4.  The  same  trends  are  shown  for  the  SANDY  con¬ 
troller  as  for  the  LQG  controller,  namely  that  transition  between  the  phases  can  be 
seen.  The  SANDY  controller  also  provides  acceptable  performance  by  maintaining 
contact  after  initial  impact  and  then  successfully  applying  the  commanded  50  gf 
while  tracking  the  unknown  surface. 

The  corresponding  SANDY  end-point  plot  is  shown  in  Figure  15.5.  The  same 
general  trends  for  the  reference  are  visible  here  as  in  the  LQG  test.  There  is  a  little 
more  movement  to  the  left  and  right  on  the  surface  after  initial  contact  for  the 
SANDY  controller,  but  after  that  the  line  is  followed  well.  The  final  phase  of  position 
control  involves  the  same  line  reference  from  the  end-point  location  at  the  start  of 
Phase  6  to  the  initial  starting  location. 

The  next  check  of  the  complete  task  is  to  follow  an  unknown  arc  surface  at  an 
unknown  location.  First,  the  LQG  controller  is  applied  at  the  same  1  cm/s  tracking 


Retract,  SAN,  .005  offset,  1  s  hold,  1  s  wall,  unk  line,  .7  cm  per  sec 


Figure  15.4-  SANDY,  Unknown  Line,  Strip  Chart 


speed  and  50  gf  commanded  force  level  for  10  seconds.  Figure  15.6  shows  that  con¬ 
tact  is  made  at  about  2.5  seconds,  and  the  same  transitions  between  the  phases  are  vis- 
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ible.  Once  the  constrained  motion  phase  starts,  the  force  is  maintained  at  the 
commanded  level  while  the  surface  is  followed.  Finally,  the  tool  is  returned  to  the  ini¬ 
tial  position  through  pure  position  control. 


x_ep(m) 

xep-yep  with  50  gf  cmd  (x=.5sec)  SAN,  Unk  Line  Task,  ,7  cm  per  sec 


Figure  15.5-  SANDY,  Unknown  Line,  End-Point  Position 
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Retract,  LOG,  .005  offset,  1  s  hold,  1  s  wall,  unk  arc,  1  cm  per  sec 


Figure  15.6-  LQG,  Unknown  Arc,  Strip  Chart 
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xep-yep  with  50  gf  cmd  (x^.Ssec)  LOG,  Unk  Arc  Task,  1  cm  per  sec 

Figure  15.7-  LQG,  Unknown  Arc,  End-Point  Position 

The  corresponding  end-point  plot  is  shown  in  Figure  15.7.  Once  again,  the  refer¬ 
ence  is  into  the  surface  and  switched  to  the  location  of  initial  contact.  The  surface 
following  algorithm  then  correctly  provides  a  reference  for  following  an  arc. 


Retract,  SAN,  .005  offset,  1  s  hold,  1  s  wall,  unk  arc,  .7  cm  per  sec 


Figure  15.8-  SANDY,  Unknown  Arc,  Strip  Chart 

For  completeness,  the  SANDY  controller  is  applied  to  the  control  structure  for  an 
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unknown  arc  at  an  unknown  location.  The  experiment  is  run  for  a  50  gf  command  and 
a  rate  of  0.7  cm/s  for  the  surface  following  algorithm  for  10  seconds,  as  shown  in  Fig¬ 
ure  15.8.  The  SANDY  position  controller  moves  the  arm  a  little  more  quickly,  and 
thus  a  higher  initial  recoil  occurs.  After  transitions  between  the  phases,  the  SANDY 
controller  also  provides  good  performance  for  the  constrained  motion  phase.  Finally, 
the  end-point  contact  with  the  surface  is  ended  and  returned  to  the  starting  position. 

The  corresponding  end-point  plot  is  shown  in  Figure  15.9.  Once  again,  the  same 
trends  noticed  for  the  LQG  controller  are  present.  The  end-point  moves  and  toward 
the  surface,  makes  contact  with  the  surface,  follows  the  unknown  arc,  and  then  re¬ 
tracts  to  the  initial  starting  position. 


xep-yep  with  50  gf  cmd  (x=:.5sec)  SAN,  Unk  Arc  Task,  .7  cm  per  sec 

Figure  15.9-  SANDY,  Unknown  Arc,  End-Point  Position 

Thus,  the  complete  hybrid  task  structure  is  experimentally  verified.  The  structure 
works  for  various  unknown  surfaces  as  well  as  full  and  reduced-order  controllers. 
The  constraint  is  that  the  impact  speed  is  limited  by  the  physical  bandwidth  of  the  ac¬ 
tuators  and  the  system.  However,  the  approach  speed  in  the  initial  phase  can  always 
be  adjusted  to  provide  an  acceptable  touchdown  speed.  The  big  advantage  of  this  ap¬ 
proach  is  that  the  exact  location  and  geometry  of  a  surface  is  not  required  a  priori. 
The  only  requirement  is  that  the  general  direction  and  general  location  of  the  surface 
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be  known.  This  is  because  the  tool  does  need  to  physically  make  contact  with  the 
surface. 


16  Conclusion 


16.1  Summary 

This  research  represents  a  consolidation  of  numerous  tasks  ranging  from  devel¬ 
oping  the  force  and  position  models  of  a  two-link  flexible  manipulator  to  the  design 
and  test  of  robust  reduced-order  hybrid  position  and  force  controllers.  In  all  12  posi¬ 
tion  controllers  and  12  force  controllers  were  designed  and  experimentally  evaluated 
for  a  total  of  24  different  controller  designs.  This  does  not  include  the  combinations 
tested  for  hybrid  superposition  control. 

The  initial  goal  of  the  research  was  to  prove  that  robust  reduced-order  controllers 
could  provide  comparable  performance  with  full-order  controllers.  The  experimental 
results  for  position,  force,  hybrid  superposition,  and  hybrid  complete  optimization 
showed  that  indeed  this  is  the  case. 

Another  goal  of  the  research  was  to  develop  a  hybrid  framework  compatible  with 
a  typical  manufacturing  task.  Namely,  start  at  an  arbitrary  initial  condition,  move  to 
a  manufacturing  surface,  make  contact,  and  then  perform  a  constrained  motion  hy¬ 
brid  task  while  maintaining  a  constant  force.  Finally,  retract  from  the  surface 
gracefully  and  return  to  the  initial  position.  The  complete  hybrid  framework  devel¬ 
oped  in  this  dissertation  allows  the  aforementioned  task  to  be  accomplished  using  the 
same  controllers  throughout  the  task.  In  addition,  the  framework  allows  the  situation 
where  the  manufaeturing  workpiece  shape  and  exact  location  is  not  known. 

Connecting  both  major  thrusts  of  the  dissertation  together  involved  comparing 
the  performance  of  the  full-order  LQG  controllers  with  the  robust-redueed  order 
SANDY  controllers  in  the  complete  hybrid  control  scheme.  Once  again,  the  redueed- 
order  controllers  provided  eomparable  performance  to  the  full-order  controllers. 

16.2  Design  Lessons  Learned 

Since  this  was  an  application-oriented  dissertation,  insight  into  the  design  and  im¬ 
plementation  of  hybrid  controllers  was  obtained.  Some  lessons  learned  are  discussed 
in  the  following  section. 
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16.2.1  Hardware  Variability 

Since  the  two-link  arm  (TLA)  is  a  real  physical  device,  it  was  noticed  that  the  per¬ 
formance  varied  some  from  day  to  day.  There  are  several  reasons  for  this.  First,  the 
TLA  floats  on  a  flat  table  through  air  pads  to  provide  a  “low  friction”  environment. 
However,  depending  on  how  the  arm  was  balanced,  the  actual  friction  level  varied. 
This  was  especially  noticeable  after  the  links  were  changed  out  to  accommodate  oth¬ 
er  experiments. 

In  addition,  the  motors  are  DC  torquers  which  possess  effectively  zero  backlash. 
However,  since  an  integral  component  of  these  motors  is  a  permanent  magnet,  there 
was  always  some  residual  magnetism.  When  the  arm  was  moved,  it  had  a  tendency 
to  move  back  toward  the  initial  location  when  the  control  torques  were  removed.  Al¬ 
so,  this  effect  was  amplified  when  low  torques  were  applied. 

The  lesson  learned  here  is  that  when  comparing  different  controllers,  it  is  very 
important  to  compare  them  at  about  the  same  time.  This  assures  that  they  are  acting 
on  the  same  plant.  Of  course,  since  any  physical  system  is  actually  nonlinear,  there 
will  always  be  some  variability  in  performance.  If  the  plant  variation  is  drastic  from 
day  to  day,  a  robust  controller  can  be  designed  to  work  for  all  of  the  plant  conditions. 

16.2.2  Optimization  Observations 

Several  very  important  design  observations  were  made  regarding  controller  opti¬ 
mization  for  flexible  structures.  First,  when  optimizing  controllers  for  a  flexible 
structure,  it  is  very  important  to  have  at  least  one  plant  condition  to  excite  all  frequen¬ 
cies.  Namely,  have  a  plant  with  a  white  noise  input  directly  into  the  control  input, 
similar  to  the  LQG/LTR  fictitious  noise  input.  In  early  optimization  attempts  when 
only  a  filtered  command  and  a  pure  step  input  was  used,  the  optimization  tended  to 
ignore  the  high  frequency  modes.  Thus,  the  rigid  body  modes  were  optimized  and 
gave  good  high  bandwidth  performance,  but  they  excited  some  of  the  flexible  modes. 
Placing  the  white  noise  (impulse  input)  as  one  plant  case  allowed  the  optimization  to 
consider  the  flexible  modes  as  well  as  the  rigid  body  modes. 

Another  key  element  was  a  variable  damping  constraint.  With  only  hub  actuators 
and  sensors  available,  there  was  no  way  to  control  the  flexible  modes.  Thus,  a  uni- 
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versal  damping  constraint  could  not  be  used.  Therefore,  the  optimization  had  no  way 
to  constrain  the  damping  of  the  rigid  body  modes.  By  adding  a  damping  constraint 
below  the  first  flexible  mode  frequency  but  above  the  frequency  of  the  rigid  body 
modes,  the  optimization  could  effectively  add  damping  to  the  rigid  body  modes  with¬ 
out  trying  to  control  the  flexible  modes. 

Another  observation  is  that  the  optimization  worked  better  with  a  good  initial 
guess,  as  logic  would  dictate.  Thus,  it  is  important  to  use  all  the  information  available 
for  the  initial  guess.  Sources  for  initial  guesses  include  simple  PID  or  classical 
controllers. 

Finally,  there  was  a  trade-off  between  the  number  of  plants  chosen  for  a  robust 
design  and  the  computation  time.  The  designer  should  consider  adding  only  enough 
plants  to  cover  the  design  objective.  Plants  that  are  between  two  other  plant  cases 
should  be  left  out.  For  example,  if  the  designer  has  models  for  a  light,  medium,  and 
heavy  tip,  only  the  light  and  heavy  models  need  to  be  used. 

16.2.3  Logical  Design  Steps 

Many  times,  one  of  the  most  vexing  questions  facing  a  designer  is  “Where  do  I 
start?”  The  experience  gained  in  this  dissertation  provides  useful  guidelines  for  hy¬ 
brid  position  and  force  control  designs. 

1.  Design  a  single  plant  position  controller. 

2.  Design  a  robust  position  controller  starting  from  the  original  position  control¬ 
ler  (if  needed). 

3.  Design  a  single  plant  force  controller. 

4.  Design  a  robust  force  controller  starting  from  the  original  force  controller  (if 
needed). 

5.  Design  a  hybrid  position  and  force  controller  starting  from  an  existing  posi¬ 
tion  and  force  controller  design. 

6.  Design  a  robust  reduced-order  hybrid  position  and  force  controller  starting 
from  the  original  hybrid  position  and  force  controller  design. 
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16.3  Recommendations  for  Future  Research 

One  of  the  interesting  things  about  conducting  dissertation  research  is  that  nor¬ 
mally  for  every  question  answered,  several  more  arise.  Given  a  finite  amount  of  time, 
there  are  only  so  many  things  which  can  be  accomplished  in  one  dissertation.  Bearing 
that  in  mind,  here  are  some  recommendations  for  future  research. 

First,  end-point  position  sensing  should  be  added  to  the  design  effort.  This  would 
allow  both  collocated  and  non-collocated  control  of  position.  Keeping  in  mind  that 
with  a  flexible  manipulator,  the  end-point  position  can  only  be  approximated  through 
the  forward  kinematics  makes  this  a  logical  extension.  In  addition,  attempting  hybrid 
control  on  more  flexible  links  would  provide  a  more  challenging  design  problem. 

Sensor  blending  between  the  end-point  sensor  and  the  hub  angle  sensors  would 
also  provide  interesting  research.  Also,  multiple  sensors  open  the  door  for  fault  de¬ 
tection  and  isolation  research  on  a  two-link  flexible  manipulator. 

Finally,  an  area  that  provides  some  interesting  questions  is  in  terms  of  the  inter¬ 
action  between  the  force  and  position  controllers.  There  is  a  trade-off  between  the 
force  and  position  controller  interaction  in  terms  of  bandwidth.  Bandwidth  separation 
was  used  to  provide  acceptable  interaction  between  the  force  and  position  controllers. 
The  force  control  was  given  higher  bandwidth  to  maintain  a  desired  force  level  while 
in  contact  with  the  surface.  However,  it  was  also  discovered  that  for  sharp  inverted 
arcs  that  the  force  controller  would  apply  torques  which  would  tend  to  move  the  end¬ 
point  and  the  position  controller  did  not  have  the  bandwidth  to  hold  the  end-point  at 
the  desired  location.  Somehow,  if  the  relation  between  the  two  bandwidths  could  be 
quantified  for  different  shapes,  it  would  aid  in  future  designs. 
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Appendix  A  Arm  Modeling  Details 

A.l  Assumptions 

To  simplify  the  mathematical  models,  several  assumptions  are  made.  First,  the  air 
cushions  provide  torsional  support  and  provide  a  planar  motion  for  the  manipulator. 
Also,  each  joint  is  assumed  frictionless  with  zero  backlash  (ideal  pinned  joint  action). 
In  addition,  no  frictional  effects  are  included  for  the  elbow  and  tip  pads  due  to  the  air 
cushion.  Lumped  spring-mass-damper  (LSMD)  models  assume  linear  spring  and 
damper  elements,  and  therefore  are  valid  only  for  small  deflections  about  a  nominal 
link  condition.  From  an  electrical  standpoint,  the  amplifiers  are  assumed  to  provide 
constant  amplification. 

A.2  Parameters 

Table  A.  1  shows  the  TLA  parameters  required  by  the  LSMD  manipulator  model. 
Table  A.2  lists  the  parameters  and  their  descriptions  [56]. 


Table  A.l  -TLA  Manipulator  Parameters 


Location 

Link  1 

Link  2 

Units 

Hub 

Ih2=5. 1099x10-'^ 

kg-m^ 

Hub 

mj,i=1.1618 

mh2=0.5745 

kg 

Hub 

Chi=5.5959xl0'^ 

Ch2=0.0236 

m 

Hub 

2  =0.05 842 

Oh2=0.1143 

m 

Link 

mLi=0.1493 

mL2=0.4155 

kg 

Link 

Li=0.4699 

m 

Link 

N/ri? 

ti=0.003175 

t2=0.003175 

m 

Link 

hi=0.0381 

h2=0.0381 

m 

Tip 

Iti=0.0058 

kg-m^ 

Tip 

m(i=2.0755 

m(2=0.35 

kg 

Tip 

Cti=4.3807xl0-^ 

Ct2=4.3807xl0''^ 

m 

Tip 

Otj=0.0063 

Ot2=0.0063 

m 

For  the  two-link  manipulator  used  in  this  dissertation,  values  for  the  physical  pa- 
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rameters  were  experimentally  determined  [22].  Some  values  were  measured  with  a 
precision  ruler,  while  others  were  based  on  calculations  from  measurements.  In  ad¬ 
dition,  the  elastic  parameter  k  was  determined  from  measurement  of  tip  deflection 
from  a  known  force.  The  damping  parameter  b  was  determined  from  an  iterative  ad¬ 
justment  to  make  the  simulation  output  match  the  experimental  output  in  terms  of 
fundamental  damped  frequency  and  decay  rate.  The  damping  coefficient  varies  with 
the  number  of  links,  and  the  only  method  available  to  obtain  the  correct  vetlue  is 
through  experimental  tuning.  Also,  all  the  damping  characteristics  were  assumed  to 
be  equal  at  each  joint  of  the  LSMD  model  to  make  the  mathematics  easier. 


Table  A.2  -  LSMD  Parameters 


Parameter 

Description 

number  of  elements  per  link 

base  rotation  of  link  i 

jth  elem  rotational  deflection 

^ep 

global  X  Cartesian  tip  position 

Yep 

global  y  Cartesian  tip  position 

local  X  Cartesian  tip  position 

local  X  Cartesian  tip  position 

Ohi 

distance  from  shaft  i  to  link  i  attachment  to  hub 

Oti 

distance  from  tip  attachment  point  of  link  i  to 
shaft  i+1 

^hi 

distance  from  shaft  i  to  the  c.o.m  of  hub  i 

Cti 

distance  from  motor  shaft  i  +  1  to  c.o.m  of  tip  i 

li 

length  of  element  for  link  i 

Ihi 

Inertia  of  hub  i  about  its  c.o.m 

Iti 

Inertia  of  tip  i  about  its  c.o.m 

Ill 

Inertia  of  link  i  elements  about  their  c.o.m’s 

mhi 

hub  i  mass 

mti 

tip  i  mass 

mii 

link  i  element  masses 

^Li 

total  link  mass 

ki 

link  i  element  torsional  stiffness 

l>i 

link  i  element  torsional  damping 

'Ci 

motor  torque  at  joint  i 
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A.3  Nonlinear  Equations  of  Motion 

The  nonlinear  equations  of  motion  developed  by  Oakley  [56]  used  Kane’s  meth¬ 
od  [30].  It  is  convenient  to  express  a  generalized  speed  vector  u  as: 


ii  -  1 

k=  1 

flj-l 

(A.1) 

S  ®2  +  ^  ^2k^2  ~  •••> 

k=l  k=l 

(A.2) 

[«!>  •••»  +  p  •••> 

(A.3) 

where  Oi  is  the  shoulder  motor  velocity,  02  is  the  elbow  motor  velocity,  is  a  sublink 
velocity  for  link  1  and  ^j^is  a  sublink  velocity  for  link  2.  Additionally,  the  z  vector 
contains  the  position  of  the  manipulator  and  the  t  vector  contains  the  motor  torque. 

z=  ^11, ...»  ®2’ ^21’ •••»  (A.4) 

X  =  [tj,  x^]  ^  (A.5) 

The  complete  non-linear  matrix  equation  for  a  two-link  manipulator  is  therefore 
represented  by  the  following  equation: 

M  (z)  u  +  V{z,u)  +Rz  +  Kz  =  Tx  (A.6) 

where  M  possesses  the  mass  and  inertia  properties,  V  contains  the  centripetal  and  Co¬ 
riolis  terms,  R  possesses  the  Coulomb  torsional  damping,  and  K  contains  the  torsion¬ 
al  stiffness.  Also,  T  is  the  torque  input  distribution  matrix. 

A.4  Linearized  Two-Link  Equations  of  Motion 

Linearization  may  occur  about  any  joint  condition  with  zero  link  deflections  and 
all  velocity  states  equal  to  zero.  Application  of  a  Taylor’s  series  expansion  about  the 
non-linear  equations  of  motion  in  Equation  (A.6)  produces  the  desired  linearization. 
The  resulting  linearized  equation  of  motion  is: 

MAz+RAz  +  KAz  =  TAx  (A.7) 

where  M  is  a  Taylor  expansion  of  M  about  the  chosen  linearization  condition.  V 
from  the  non-linear  equation  disappears  since  it  is  a  function  of  velocity  squared,  and 
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velocity  is  assumed  small  near  the  linearization  state. 

It  is  convenient  to  place  the  linearized  equations  of  motion  in  the  state-space  form 
of  the  following  equations. 


M  0 

d 

At 

-R  -K 

^5 

+ 

T 

0  I 

dt 

Az 

I 

o 

_ 1 

0 

At 


dt 


MM 

M  0 

-1 

-R 

-K 

-1- 

MO 

-1 

T 

Az 

0  I 

I 

0 

0  / 

0 

X  =  Ax  +  BAx 
y  =  Cx-¥  DAx 


-1 

r 

1 

A  = 

MO 

-R 

-K 

0  I 

I 

0 

B  = 


n-1 

) 

T 

r 

0 

X  =  [a  A  T 

~  [Az  Az 


(A.8) 


At  (A.9) 

(A.  10) 
(A.  11) 

(A.12) 

(A.13) 
(A.  14) 


Matlab  m-files  tlapar.m  and  tlagen.m  achieve  the  linearization  using  this  scheme.  Al¬ 
so,  the  m-files  are  converted  to  MATRIXx  form  in  tlapar.mws  and  tlagen.mws  to 
achieve  the  linearization.  The  tlapar.m  file  contains  the  parameters  which  are  specific 
to  the  manipulator  model,  such  as  the  hub  inertia,  as  well  as  containing  the  number 
of  elements  per  link  desired.  After  loading  the  parameters,  running  the  tlagen.m  file 
actually  generates  the  new  A  and  B  matrices,  and  calls  them  Abar  and  Bbar.  The  in¬ 
puts  to  the  model  is  the  torque  vector  and  the  output  contains  the  states  as  well  as  x 
and  y  endpoint  positions.  The  original  work  for  the  m-files  was  done  by  Evers  [22]. 


A.5  Non-Linear  Two-Link  6  DOF  Dynamic  Equations 

To  provide  a  feel  for  the  non-lineeir  dynamics,  this  section  provides  the  develop¬ 
ment  of  a  6  DOF  dynamic  model  for  the  two-link  manipulator  [22].  Three  elements 
per  link  provide  the  basis  for  a  6  DOF  model.  Using  the  basis  of  3  identical  elements 
for  link  1  and  3  identical  elements  for  link  2  results  in  the  following  mass  center 
velocities: 


2 


(A.  15) 


V;  = 
‘n 


2  2 
“  ^h\^l 


2 


(A.16) 


V,  = 


2  2  2/^/lV 


'*’■■■  (A.18) 

+  2mjM2  (o^jj  +  /I)  /Icos  (-^ij)  +  M1M3  (0^1  +  /I)  /Icos  (-^?ii  -^12)  +  ••• 

+  M2M3^1cOS  (-<3'i2) 

2  2  2  2  2  2 
^rl  “  ^1  W2/I  +  u^  (/I  +  +  ...  (A. 19) 

+  2mjM3(<7^j +/1)  (/I +o^i  +  c^j)cos  (-?n-?i2)  +••• 

+  2MjM2(o^j  + /I) /I  cos  (-?ii)  +2M2M3/I  (/I  +Oji  +^fl)  (-^12) 

2  2  2222  222 
v^2  =  “1  (O/il  +  ^1)  +“2^1  +M3(/1+Ojj)  +M4C^2+---  (A.20) 

+  2mjMj(c>  + /I)  (/I  +  o  )  cos  (-^  -^)+... 

+  2MjM2^1  (^1  +  0^1)  cos  (-^jj)  +  2M2M3/I  (/I  +  Ojj)  cos  (-^12)  ■*■  ••• 

+  2u-^U^{Of^-^  +  /1)  C^2COS  (~^j2~^j2~^2^  +  ... 

+  2u2U^llCf^2^0S  (^J2  -  ^2)  +  2M3M4  (/I  +  O^j)  Cf^2^0S  (-^2) 


%  =  M?(OM+^l)"  +  “2^l"  +  “3(^l+^n)"  +  «4(‘>A2  +  |)^+-(A-21) 
+  2MjM2  (<?^j  +  /I)  /Icos  (-^jj)  +  2M2M3/I  (/I  +  Ojj)  cos  (-^12)  +  ••• 

+  {2u^u^io^^  +  n)  (/l+o,j)cos  (-^11-«?12))  +••• 


—  Mj(o^j+/l)  +M2/I  +  (<?^2  ■*■  ^2)  +...(A.22) 

2^/2^ 

.  +M2I— J  +  2MjM2  (0;,J  + /I) /Icos  +  ... 

.  +  2MjM3  (o^j  +  /I)  (/I  +  O^j)  cos  (-^11  -  ^12)  ■*■••• 

.  +  2MjM4(o^j+/1)  (/2  +  0^2)  ■■' 

.  +MiM5(o^1  +/1)/2C0S  (-^ii-?12“^2“^2i)  +  ••• 

.  +  2M2M3/I  (Ojj  +  /I)  cos  ■*■••• 

.  +2M2M4/I  (0^2  + ^2)  cos  (-^12-^2)  ■*■••• 

.  +  U2U^lll2cOS  (-^j2~^2  “  ^21)  "^  ••• 

.  +  2M3M4 (Ojj+/l)  (/2  +  0^2)  cos  ("^2)  ■^'  •  •  • 

.  +  M3M5  (Ojj  +11)  /2cOS  (-  (^2  “  ^21)  ■*■  ^4^5  (^/!2  /2cOS  (-^21^ 

=  M|  (o^j  +  /  I)  +  M2/ 1  +  {li  +  o ^^)  +  M4  ^^fi2  '*’  ^"  •  •  •  (A. 23) 

2  1  if  12\^ 

.+u^{l2)  J  +  2mjM2 (O/jj  + /I) /Icos  (-^jj)  +  ... 

.  +  2MjM3(c>^j+/1)  (/1+O^j)  cos  (~  ^jj  ~  ^12^  +  ... 

. +2MjM4(o^j +/1)  (/2  +  o^2)‘^os(-^ii-^12~^2) 

.  +  2MjM3(o^j  +  /1)  /2C0S  (~5jj~^j2~^2~^21^  '*’  '"■ 

.  +  MjMg(o^j  +  /1)  /2cOS  (~^jj  ~  q\2~  ^2~  ^2\  ~  ^22)  "^  ••• 

.  +  2U2U2II  (o^j  +  11)  cos  (-^12)  2m2W4^1  (Of^2  +  ^2)  cos  (- ^J2-  ^2^  ■*■  ••• 

.  +2M2M5M/2COS  (-^|2-^2“^2i)  ■'■••• 

.  +  M2Mg/l/2cOS  (-^i2~^2“^21  ~  ^ll)  ••• 

.  +  2M3M4  (o^j  +  /I)  (/2  +  0^2)  cos  (-^2)  ■•■••• 

.  +  2M3M3  (o^j  +  11)  /2cOS  (-  ^2  -  q2\)  +  2M4M5  {0^2  +  ^2)  /2cOS  (-^2l) 
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...  +  {II  +  Ojj)  /2C0S  (-  ^22“^2“^2i)  ’'■••• 

2 

...  +  (0^2  +  ^2) /2cOS  (- ^21  “^22^  +M5M^/2  COS  (-^22) 


4  =  m?(O;,i+/1)^  +  M2^1^  +  M3(/1  +  0ji)^  +  M4(O;,2  +  ^2)^+...(A.24) 

2  2  2  2 

...+U^{12)  +  Mg  (/2  +  0^2  +  <^;2)  +  2MjM2  (o^j  + /I) /Icos  (-^jj)  +  ... 

. . .  +  2m jMg  j  +  /1)  (/1+Ojj)  cos  (~4'ii~^12^  +  ... 

... +2MjM4(o^j +/1)  (/2  +  0^2)<^<^S  (-^11  “^12  “^2)  +••• 

...  +2MjM5(0;jJ  +/l)/2cos(-^jj  -^j2“^2“^2i)  +  ••• 

...  +2MjMg(0;jj  +/1)  (/2  +  0^2  +  ^r2)  cos  (-?11-^12-^'2“?21~^22)  +  ••• 

...  +  2M2M3/I  (Ojj  +  /  I)  cos  (-^12)  +  2M2M4/I  (0^2  +  ^2)  cos  (-^?12-  ^2)  ■*■••• 
...  +  2M2M3/I/2COS  (-^]2~?2“^2i)  ■*■••• 

...  +  2M2Mg/l  {12  +  0^2  +  ^<2^  COS  {-q^2~^2~^2l  ~  ^ll)  '^  ■■■ 

...  +  2M3M4  (Ojj  +  /I)  {12  +  0^2)  00s  ("^2)  ■!■••• 

...  +  2M3M3  (Ojj  +  /I)  /2cOS  (-  ^2  “  ^2l)  ^^4^5  i^hl  '*'  ^2cOS  (-^21) 

. . .  +  2  M3  Mg  (/I  +  <2jj)  {12  +  0^2  COS  (—  ^22~^2~^21^ 

...  +  2M4Mg  (0^2  +  ^2)  (/2  +  0?2  +  C?2)  cos  (-^21  "^22^  ■*■••• 

. . .  +  2u^u^l2  {12  +  0^2  +  ^j2)  cos  (-^22^ 


The  current  equations  are  unwieldy,  so  generalized  inertia  terms  are  introduced. 
The  Lagrangian  development  of  the  equations  of  motion  to  follow  uses  these  gener¬ 
alized  inertia  terms. 


h\  -  +  +^/i  + 

...  +  (2mjj  +m^j  +  m^2  +  3m^2  +  ^f2) 


(A.25) 
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II  Y  2 

■^22-^n{2)  +  +  (A.26) 

fny  2 

■^33  -  J  +^/l +"*?!  ■'■•••  (A.27) 

.+ (m/,2  +  m;2  +  m,2)  (^1+^n)^ 

2  /2V  2 

4  =  "*Zi2^/j2  ■'■  ^2  ■'■  "*Z2l^^;z2  +  y  J  +  ^Z2  ■*■  ^^^12  +  ^t2)  (^/i2  +  (A.28) 

•^55  “  ■*’-^Z2'*'  {fni2  +  fn^2)l'^  (A.29) 

/'/2  V  2 

•^66  -  '”z2l^‘2  J  +^Z2''''”f2(^2  +  ^f2‘'‘^r2)  ■*■  ^r2  (A.30) 

/i2  =  ^  [  (3m^j  +  2m^j  +  2m^2  +  6^/2  ■*■  ^^t2)  +  ^1)  ^1]  (A. 31) 

*^13  =  I  (^/i^l  +  2m^j  (/I  +  Ojj  +  Cjj) )  (o^j  +  /I)  +  ...  (A.32) 

+  \  (2m^2  +  6'”z2  +  +  “^fl) 

A4  “  '^^^h2^h2'^'^^li^^h2'^ '*’■■■  (A.33) 

+  -  (4m^2  +  2m^2)  ('2/j2  +  ^2)  (o^j  +  /I) 

•/^15  =  ^  [  (3m^2  +  2m,2)  (o/,i  +  /l)/l]  (A.34) 

*^16  “  2  ^  (^Z2^^  ■'■  ^^i2  *^j2  ^<2^  )  (<2/,i+/l)] 


(A.35) 
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•^23  “  ^(wj^j/1 +2m^j  (/I +o^j +Cjj))/1  +  ...  (A.36) 

*^24  “  '*'•••  (A.37) 

.  +  ^  (4m;2  +  2"*/2)  (^/i2  + 

^25  =  ^  [  (3m;2)  /2  (0;,2  +  ^2)  +  2m, 2/2]  11  (A.38) 

•^26  =  1 1  (m;2/2  +  2m, 2  (/2  +  0,2  +  c,2) )  (/I)  ]  (A.39) 

■^34  “  ■•■  2^/2(^A2  ■*■  "2  ])  ■•■•••  (A.40) 

■*■  \  (‘^^11  +  2^,2)  (0^2  +  ^2)  (<?,J  +  /  I) 

y35  =  ^  [  (3'”/2  +  2m,2)  (o^j  +  /l)/2]  (A.41) 

•^36  =  ^  [  (»J/2^2  +  2m,2  (/2  +  0,2  +  0,2) )  (0,1  +  /I)  ]  (A.42) 

^45  =  ^  [  (3m;2  +  2m,2)  (0^2  +  ^2)  /2]  (A.43) 

•^46  =  ^  [  ('«/2^2  +  2m,2  (/2  +  0,2  +  0,2) )  (0^2  +  ^2)  ]  (A.44) 

•^56  “  2  ^  ("*/2^2  +  2m,2  (/2  +  £>,2  +  0,2) )  (/2)]  (A.45) 
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The  kinetic  energy  expression  for  the  6  DOF  system  can  be  expressed  using  the 


generalized  inertia  terms  as  follows; 


.2  ^22  .2  *^33  .2  *^44  .2  ^55  .22 

Y«i  +  -y«2  + Y^3  +  y4  +  y“5  + 


^66  .2 

+ 

2  6 


...  +  •^13MiW3COS  (Mj  -  Mj)  +  .. 

...  +7j4MjM4  COS  (Mj  -M4)  +7j5MjM5COS  (Mj  -M5)  +  .. 
...  +  7jgMjMgCOS  (Mj  -  Mg)  +  723^2^3^®®  (^2  ~  “3) 

...  +724M2M4COS  (M2-M4)  +725M2M5COS  (M2 -Mg)  +  .. 

...  +  72gM2MgCOS  (M2  -  Mg)  +  734M3M4COS  (Mj  -  M4)  +  .. 
...  +  73gM3MgCOS  (M3  -  Mg)  +  73gM3MgCOS  (M3  -  Mg)  +  .. 
...  +74gM4MgCOS  (U^-U^)  +  J^^li^U^COS  (M4-Mg)  +  .. 


(A.46) 


...  +  7ggMgMgCOS  (Mg  -  Mg) 


The  potential  energy  in  the  torsional  springs  is: 


T7  1 7  /  \  2  1  ,  .  ,  2  1  ,  ,  .2  (A.47) 

y=-ki(M2-Mi)  +-/:j(M3-M2)  +2^2(“5““4)  +-" 

If  /  ^2 

+  9^2  (“6  ““5) 


Lagrange’s  expression  in  the  following  equation  develops  the  equations  of  mo- 
/ 

tion.  Here,  Q-  are  the  generalized  forces  due  to  torsional  damping. 


dt\du-)  du^ 


(A.48) 


Applying  the  above  equation  for  the  6  DOF  system  yields  the  equations  of  motion 
for  the  system  as  follows. 

73jMj  +7J2M2COS  (-^11)  +7J3M3COS  (-^n  -^12)  ••• 


(A.49) 


r 
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...  +7j4M4COS  (-^11  -^12-^2)  (-^jj  -^j2-?2-^2i)  +  "• 

...  +7jgMgC0S  ~  Q\2~  ^2~  ^2\  ~^27)  '*'  •" 

...+7i2M2sin  (-^11)  +7j3M3sin(-^jj-^j2)  +••• 

...+7l4M4Sin(-^jj-^j2-?2)  +-^i5«5Sin(-^jj-^j2-g2-^2l)  +••• 

... ("“^11  ~^12~^2~^21  “^22^  =  ... 

...  =  +  Z?j  (M2 - Mj)  +  7’j 

722^2 +  -^21  “1^®^  +  723M3COS  (-^j2)  +••• 

...  +  724M4COS  (— ^12  —  ^2)  ■*■'^25*^5^^^  (“  ^12  ~  ^2  ~  ^21)  +  ... 

. . .  +  J^^U^COS  (-  ^12  -  ?2  -  ^21  -  ^22)  +  •  •  • 

2  2 

...-72jM2Sin  (-^jj)  +722M3Sin  (-^J2)  +••• 

2  2 

...+724M4Sin  (-^i2“?2)  +>^25“5^^^  (“  ^12  “  ^2  “  ^2l)  +••• 

•••  ■’■'^26“6^^'^  ^~^12~^2“^21  “^22^  “  "• 

...  =  +^i^l2“^j  (“2“*^!^  ■*■^1  i^2~^2^ 

^ 33  ^3  *^31^1  (  ^11  ^12^  '1’7 32^2  COS  (  12)  ...  (A. 5 1) 

...  +734M4COS  (-^2)  +  •^35^5 COS  (-^2“^2i)  +  ••• 

...  +  COS  (~  ^2  ~  ^21~^22^  +  ... 

2  2 
...-73jMjSin  (-^11-^22)  -^32“2^^’^  (-^12)  +  ••• 

2  2 

... +734M4Sin(-^2)  +>^35«5Sin(-g2-^2l)  +••• 

. . .  +  73gMjsin  (-^2-^21  -  ^22)  =  •  •  • 


(A.50) 


I 
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...  =  (*^3  “  “2^  “^2 

+  (-^2  “^11  “^12)  (-^2~?12)  +•• 

...  +743M3COS  (-^2)  +-^45“5C0S  (-^21)  +  ••• 

•••  +>^46WgCOS  (-^21“^22)  '*'••• 

•• -->^41  “1  sin  (-911-^22-^2)  -'^42“2sin(-?i2-92)  +••• 

•••-•^43«3Sin  (-^2)  +‘^45“5Sin  (-^21)  +  ••• 

... +746MgSin(-^21-922)  =••• 

•••  =  M12+  (^2)  (“5-^4)  +^2 


“^si^^i^^s  (  ^2  921  912)  +  ••• 

...  4“  1/32^2^^^  (  92  9i2  92i)  ./33W3COS  (  ^21  92)  **’ 

...  +/34M^C0S  (~^2i)  +>^^^^3008  (—^22)  '*’  ••• 

.  ..-/51MJ  sin  (-^11-^22  -  92  -  92i)  -  •^52«2Sin  (-  -q^-q^^)+  .. 

2  2 

•••-•^53“3Sin  (-^2-92i)  -'^54“4Sin  (-92l)  +  ••• 

...+/56«6Sin(-922)  =  - 

...  =  — ^2^12  ■*■  ^29i2  —  ^2  (^5  —  *^4)  '*’ ^2  (*^6  —  ^5) 

J(U^  +  y^jlijCOS  (-^ Jj  -  ^2  -  921  -  9i2  -  922)  ■*■••• 

...  +7g2«2nos  (-^21-922-92- 912)  +-^63“3cns  (-^21-922-92)  +  ••> 
•••  +'^64“4C0S  (-^21-922)  ■*’‘^65^5^ns  (“^22)  •” 

•••— '^ei^l  si'^  (~^ii~^12~922- 92  —  921)  ■*■  ••• 


(A.52) 


(A.53) 


(A.54) 
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...  ^J2  ^21  ^22)  ‘^63^3^^'^(~^2“^21~^22)‘*'-" 

•••"'^64*^4^^°  (“^21~^22)  ^“‘?22)  =  ••• 

...  =  -/:2^22+“^2(“6““5) 

A.6  Closed-Form  Linearized  Equations  of  Motion 

The  complete  closed-form  set  of  matrices  for  the  linearized  model  are  found  in 
Oakley’s  work.  The  M  on  p.  259  of  the  dissertation  is  equivalent  to  (for  the  6  DOF 
model): 

M  = 

where 

*^11  A2  -^13 

■^11  ~  >^21  ^22  '^23  (A.56) 

/3I  '^32  ■^33_ 

•^14  hs  *^16 

^12  =  J24  ^25  ^26  (A.57) 

/34  '^35  -^36 

•^44  -^45  "^46 

^22  -  J54  J55  (A.58) 

/64  hi  ■^66 

The  terms  were  checked  and  were  found  to  be  in  agreement  with  the  exception  of 
two  typographical  errors  [22].  The  corrections  to  p.260  of  the  dissertation  are  as 
follows: 
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where 
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The  second  typographical  error  on  p.260  should  read: 

f 

ell  0  .  0 

0  ell  0  .  0 

^1  ^1  ...  ...  ...  ... 


M 


linkldiagll 


.  0  ... 

.  0  enl  0 

0  .  0  enn ) 


(A.59) 


rty 

ell  =  +  +  £  mj.  (o^i+/l)^ 

(A.60) 

I'l  =  2 

e22  =  -m  11^  +  V  m,  ./l^ 

4  12  ^  I'l 

i,  =  3 

(A.61) 

1  2  2 
enl  =  -m  11  +m.  11 

4  l(n,-l) 

(A.62) 

1  n2 

enn  =  -m  11 

(A.63) 

(A.64) 


where 


^2 

(A.65) 

jj  =  2 

«2 

e22  =  im  12^  +  Y  m,./2^ 

4  22  ^  “2 

(A.66) 

1^  =  3 

1  2  2 
enl  =  7m  /2  +  m,  /2 

4  2(n2-l)  ^"2 

(A.67) 

1  7o2 

enn  =  7/7?  /2 

4  2/i2 

(A.68) 

Appendix  B  -  Additional  SANDY  Double  Angle  and  FED  Plots 

This  appendix  provides  a  further  reference  for  the  SANDY  double  angle  and  PID 
designs.  Specifically,  the  plots  for  trajectories  1  and  3,  described  in  Chapter  5,  are 
displayed  for  the  PID  controllers.  Next,  the  SANDY  controller  for  trajectories  1  and 
3  are  displayed.  Finally,  PID  and  SANDY  are  compared  for  trajectories  1  and  3.  In 
all  the  plots  with  volts  listed,  the  conversion  for  Tj  volts  to  Nm  is  1/1.9208  and  the 
conversion  for  X2  volts  to  Nm  is  1/8.045.  Also,  all  the  angle  readings  are  relative  to 
the  starting  positions.  The  choppiness  on  the  angle  plots  is  due  to  optical  encoder 
quantization. 

B.l  PID  Plots 
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PID  Experimental  Run ,  Traj  1 

Figure  B.l-  PID  Experimental  Run,  Trajectory  1 
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PID  Experimental,  Traj  3 


Figure  B.4-  PID  Experimental  Run,  Trajectory  3 


Figure  B.5-  PID  Experimental  vs.  Simulation,  Trajectory  3,  Gj 
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Figure  B.7-  SANDY  Experimental  Run,  Trajectory  1 
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Figure  B.8-  SANDY  Experimental  vs.  Simulation,  Trajectory  1,  0| 
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Figure  B.15-  SANDY  vs.  PID  Experimental,  Trajectory  3,  Sj 
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Figure  B.16-  SANDY  vs.  PID  Experimental,  Trajectory  3, 02 


Appendix  C  -  Additional  LQG-Class  Plots 


This  appendix  provides  a  further  reference  for  the  LQG-class  controllers.  Those 
displayed  here  are  LQG/LTR,  LTR,  7th-order  LQG/LTR,  and  7th  order  LTR  control¬ 
lers.  Specifically,  the  plots  for  trajectories  1  and  3,  described  in  Chapter  5,  are 
displayed  for  the  LQG/LTR  controller.  Next,  the  LTR  controller  response  for  trajec¬ 
tories  1  and  3  are  displayed.  Then  the  7th-order  controller  results  are  listed.  Finally, 
PID  and  SANDY  are  compared  with  all  the  LQG-class  controllers  for  trajectories  1 
and  3.  In  all  the  plots  with  volts  listed,  the  conversion  for  T|  volts  to  Nm  is  1/1.9208 
and  the  conversion  for  T2  volts  to  Nm  is  1/8.045.  The  choppiness  on  some  of  the  angle 
plots  is  due  to  quantization  of  the  angle  encoders. 

C.1  LQG/LTR  Plots 
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Figure  C.l-  LQG/LTR  Experimental  Run,  Trajectory  1 
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Figure  C.2-  LQG/LTR  Experimental  vs.  Simulation,  Trajectory  1,  Oj 


Figure  C.6-  LQG/LTR  Experimental  vs.  Simulation,  Trajectory  3,  9 


Figure  C.9-  LTR  Experimental  vs.  Simulation,  Trajectory  1, 0 


Figure  C.ll-  LTR  Experimental  vs.  Simulation,  Trajectory  3,  0 


Figure  C.14-  7th  Order  LQG/LTR  Experimental  vs.  Sim,  Trajectory  1, 0 
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Figure  C.15-  7th  Order  LQG/LTR  Experimental  vs.  Sim,  Trajectory  1, 0 
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Figure  C.16-  7th  Order  LQG/LTR  Experimental  Run,  Trajectory  3 


Figure  C.17-  7th  Order  LQG/LTR  Experimental  vs.  Simulation,  Traj  3, 0 
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Figure  C.24-  7th  Order  LTR  Experimental  vs.  Simulation,  Trajectory  3, 02 
C.5  Overall  Comparison  Plots 
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Figure  C.25-  Comparison  Experimental  Runs,  Trajectory  1,  Gj 
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Figure  C.26-  Comparison  Experimental  Runs,  Trajectory  1, 02 


Exp  Comparison,  Traj  2 


Figure  C.27-  Comparison  Experimental  Runs,  Trajectory  2,  6j 
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Figure  C.28-  Comparison  Experimental  Runs,  Trajectory  2, 6 
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Figure  C.30-  Comparison  Experimental  Runs,  Trajectory  3,  6 


Appendix  D  -  Additional  Endpoint  Tracking  Plots 

This  appendix  provides  a  further  reference  for  the  trajectories  described  in  Chap¬ 
ter  9.  Specifically,  3  and  10-second  slew  plots  for  the  line  and  arc  trajectories  are 
provided,  and  6  and  20-second  slew  plots  for  the  sine  trajectory  are  included.  The 
plots  list  the  X  and  y  endpoint  positions,  hub  angles,  and  voltage  output  for  each  ex¬ 
perimental  run.  Also  listed  on  each  plot  are  the  simulation  results.  In  all  the  plots  with 
volts  listed,  the  conversion  for  Xj  volts  to  Nm  is  1/1.9208  and  the  conversion  for  X2 
volts  to  Nm  is  1/8.045.  The  choppiness  of  some  of  the  angle  plots  is  due  to  angle  en¬ 
coder  quantization. 
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Appendix  E-  MATLAB  m.Bles 


This  appendix  contains  the  MATLAB  m.  files  used  to  generate  the  models  and 
controllers  for  this  dissertation.  There  are  three  general  categories  for  the  m.files  - 
model  generation,  SANDY  design,  and  general  design.  The  model  is  generated  with 
the  help  of  two  m.files.  First,  the  tlapar3.m  file  loads  the  desired  parameters  into  the 
matlab  workspace,  and  then  tlagen.m  file  generates  a  linearized  state-space  model 
based  on  the  loaded  parameters. 

The  SANDY  design  is  primarily  accomplished  with  the  help  of  two  m.files.  First, 
the  sandyarm.m  file  generates  a  synthesis  model  compatible  with  SANDY.  Next, 
the  synthesis  model  is  used  in  sanarimn.m  to  generate  a  SANDY  data  file. 

The  general  design  files  are  used  in  LQG  design  and  also  controller  analysis.  Spe¬ 
cifically,  Iqgarmdesign.m  is  used  to  the  design  and  reduce  the  LQG-class 
controllers.  It  also  analyzes  the  controller  bandwidths.  The  bw.m  file  analyzes  the 
bandwidths  of  the  PID  and  SANDY  controllers. 


E.l  Model  Generation  m.files 

The  following  two  m.  files  are  used  to  generate  the  lumped-spring-mass-damper 
model  for  the  two-link  flexible  manipulator. 


E.1.1  tlapar3.m 


%  function 

[Ih  1 ,11 1  ,It  1  ,mhl  ,ml  1  ,mt  1  ,ch  1  ,oh  1 ,1 1  ,ct  1  ,ot  1  ,k  1  ,b  1  ,Ih2,I12,It2,mh2,ml2,mt2,ch2,oh2 
,12,ct2,ot2,k2,b2]  =  plant_param_f(nl,n2) 

%  Description:  Returns  TLA  inertial  and  geometric  parameters.  The 
%  configuration  is  for  2  flexible  links  with  no  mass  intensifiers.  The  elbow 
%  motor  stator  is  part  of  the  tip. 

%  Format:  [Ihl,Ill,Itl,mhl,mll,mtl,chl,ohl,ll,ctl,otl,kl, 

%  Ih2,I12,It2,mh2,nil2,mt2,ch2,oh2,12,ct2,ot2,k2]  = 

%  TLA_plant_param_f(nl,n2) 

%  Output  Variables 


%  Ihi  =  Inertia  of  ith  hub  about  its  mass  center 
%  Hi  =  Inertia  of  ith  link  about  its  mass  center 
%  Iti  =  Inertia  of  ith  tip  about  its  mass  center 
%  mhi  =  mass  of  ith  hub 
%  mli  =  mass  of  ith  link 
%  mti  =  mass  of  ith  tip 

%  chi  =  distance  from  ith  motor  shaft  to  ith  hub  center  of  mass 
%  cti  =  distance  from  ith  motor  shaft  to  ith  tip  center  of  mass 
%  ohi  =  distance  from  ith  motor  shaft  to  ith  hub  attatch  point  of  link 
%  oti  =  distance  from  ith  motor  shaft  to  ith  tip  attatch  point  of  link  i 
%  li  =  length  of  each  rigid  segment  of  link  i 
%  ki  =  torsional  spring  stiffness  between  ith  link  segments 
%  bi  =  torsional  viscous  damping  between  ith  link  segments 
%  Revision:  7  Oct  94,  Dave  Bossert 
%  by  Steve  Evers 
%  Variables 
nl=10 
n2=10 

thl_nom=0*pi/180 
th2_nom=90*pi/ 1 80 
%  shoulder  hub  parameters 
Ihl  =  4.1238e-03;  %kg*m^2 
mhl  =  1.1618;  %kg 
chi  =  5.5959e-05;  %m 
ohl  =  5.8420e-02;  %m 
%  link  1  parameters 

%  long  flexible  link,  no  mass  intensifiers 

mil  =  (169.5/1000)*((21-1-1.5)/21);  %kg  (measured) 

mil  =  mll/nl;  %kg 

11  =  ((21-1-1.5)/12)*0.3048;  %m  (measured) 
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ll=ll/nl;%m 

111  =  (mll*11^2)/12;  %kg*m^2 
%  link  2  parameters 

%  long  flexible  link,  no  mass  intensifiers 

nil2  =  (169.5/1000)*((20.5-1-1)/21);  %kg  (measured) 

ml2  =  ml2/n2;  %kg 

12  =  ((20.5-l-l)/12)*0.3048;  %m  (measured) 

12  =  12/n2;  %m 

112  =  (ml2*12''2)/12;  %kg*m^2 

%  tip  includes  standing  bracket,  stator  housing  and  pad 

%  roughly  experimentally  measured 

%  swing  test  with  cm  assumed  at  joint 

Itl  =  0.0058; 

mtl  =  2.0755; 

ctl  =  4.3807e-04; 

otl  =  0.0063;  %m 

%  hub  includes  hanging  bracket,  shaft  coupling  and  extender 

Ih2  =  5.1099e-04; 

mh2=  0.5745; 

ch2  =  0.0236; 

oh2  =  4.5*.0254; 

%  tip  2  parameters 

%  tip  is  composed  of  bracket,  pad,  and  link  grip  length 

%  tip  location  is  defined  as  centerline  of  bracket  base  bolts 

%  same  as  first  SLA  tip 

mt2  =  .35;  %2.9842e-01;  %kg 

ct2  =  4.3807e-04;  %m 

ot2  =  6.3500e-03;  %m 

It2  =  .003;  %1.1045e-03;  %kg*m^2 

%  beam  lateral  stiffness  properties 
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%  Experimentally  tuned 

Bill  =  5.5; 

EI12  =  EI11; 

kl  =  3*EIll*(nl*(nl-l)*(2*nl-l)/6)/(nl^2*nl*ll); 
k2  =  3*EI12*(n2*(n2-l)*(2*n2-l)/6)/(n2^2*n2*ll); 

%  beam  structural  damping 

%  experimentally  tuned  to  40th  order  plant  model  90  deg  joint 
b  =  0.01; 
bl  =  60*b/nl; 
b2  =  (60*b/n2)/2; 

%  end 

E.1.2  tlagen.m 

%  function  [Abar,Bbar,C,D,M,kl,k2,bl,b2]  =  ... 

%  plant_gen_f(nl  ,n2,thl_nom,th2_nom) 

%  Format:  [A,B.C,D]  =  TLA_plant_gen_f(nl,n2,thl_nom,th2_nom) 

% 

%  Description:  Synthesizes  a  lumped  spring/mass/damper  (LSMD)  state  space 
%  model  for  the  UW  flex-flex  TLA  in  the  following  form.  Check 

%  file  for  configuration  case: 

% 

%  d/dt{x}=A{x}-^B{u} 

%  {del_y}  =C{x} -hD{u} 

% 

%  Note:  both  {x}  and  {del_y}  are  referenced  to  the  LOCAL 

%  coordinate  system  determined  by  the  state  vector: 

%  X  =  {0  ...  0  thl_nom0  ...  0  th2_nom0  ...  0}’ 

% 

%  X  =  {dthl  dqll  ...  dql(nl-l)  dth2  dq21  ...  dq2(n2-l)  thl  ...  q2(n2-l)}’ 

%  { del_y }  =  { X  del_xep  del_yep }  ’ 
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%  {u}  =  {T1  T2}’ 

%  T1  =  shoulder  torque 
%  T2  =  elbow  torque 
%  thl  =  shoulder  joint  perturbed  base  angle 
%  th2  =  elbow  joint  perturbed  base  angle 
%  del_xep  =  X  end  point  perturbed  cartesian  position 
%  del_yep  =  y  end  point  perturbed  cartesian  position 
% 

%  ALL  UNITS  IN  METRIC  (kg-m-s) 

% 

%  Input  Variables:  nl  =  number  of  flexible  elements  for  first  link 
%  n2  =  number  of  flexible  elements  for  second  link 

%  thl_nom  =  nominal  shoulder  joint  angle  to  linearize  about 

%  th2_nom  =  nominal  elbow  joint  angle  to  linearize  about 

% 

%  Output  Variables;  [A,B,C,D]  =  state  space  realization  in  locally  prescribed 
%  coordinate  system. 

%  Revision:  7  Oct  94  by  Dave  Bossert 
%  by  Steve  Evers 

%  load  inertia/spring/mass  parameters 
% 

[Ih  1 ,11 1  ,It  1  ,mh  1  ,ml  1  ,mt  1  ,ch  1  ,ohl  ,1 1  ,ct  1  ,ot  1  ,k  1  ,b  1  ,Ih2,I12,It2,mh2,ml2,mt2,ch2,oh2 
,12,ct2,ot2,k2,b2]  =  plant_param_f(nl,n2); 

%  mass  matrices 
fori  =  2:nl-l, 

M_inertias  1 1  (i,i)  =  II 1 ; 
end 

MJnertiasl  1(1,1)  =  Ihl+Ill; 

M_inertiasll(nl,nl)  =  Ill+Itl; 
fori  =  2:n2-l. 
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M_inertias22(i,i)  =  112; 
end 

M_inertias22(l,l)  =  Ih2+I12; 

M_inertias22(n2,n2)  =  I12+It2; 

M_inertias  =  [M_inertiasll  zeros(nl,n2);zeros(n2,nl)  M_inertias22]; 
fori  =  2:nl-l, 

M_hubs_diagl  l(i,i)  =  mh2*ll'^2; 
end 

M_hubs_diagl  1(1,1)  =  mhl*chl^2+mh2*(ohl+ll)^2; 

M_hubs_diagl  l(nl,nl)  =  mh2*(ll+otl)^2; 

M_hubs_diag22  =  zeros(n2,n2); 

M_hubs_diag22(l,l)  =  mh2*ch2'^2; 

M_hubs_diag  =  [M_hubs_diagll  zeros(nl,n2);zeros(n2,nl)  M_hubs_diag22]; 
for  i  =  2:nl, 

M_linkl_diagll(i,i)  =  (l/4)*mll*11^2+(nl-i)*mll*ll''2; 
end 

%  Stanford  element  here  is  incorrect 

%M_linkl_diagl  1(1,1)  =  mll*(ohl+(ll/2))^2+nl*mll*(ohl+ll)^2; 

%  My  element 

MJinkl_diagl  1(1,1)  =  mlP(ohl+(ll/2))''2+(nl-l)*mll*(ohl+ll)^2; 
M_linkl_diag  =  [M_linkl_diagll  zeros(nl,n2);zeros(n2,nl+n2)]; 
fori  =  2:nl-l, 

M_link2_diagl  l(i,i)  =  n2*ml2*11^2; 
end 

M_link2_diagl  1(1,1)  =  n2*ml2*(ohl+ll)^2; 

M_link2_diagl  l(nl,nl)  =  n2*ml2*(ll+otl)^2; 
for  i  =  2:n2, 

M_link2_diag22(i,i)  =  (l/4)*ml2*12^2+(n2-i)*ml2*12'^2; 
end 

%  Stanford  element  here  is  incorrect 


220 


%M_link2_diag22(l,l)  =  ml2*(oh2+(12/2))^2+n2*ml2*(oh2+12)'^2; 

%  My  element 

M_link2_diag22(l,l)  =  ml2*(oh2+(12/2))''2+(n2-l)*inl2*(oh2+12)^2; 
M_link2_diag  =  [M_link2_diagll  zeros(nl,n2);zeros(n2,nl)  M_Iink2_diag22]; 
M_tipl_diag  =  zeros((nl+n2),(nl+n2)); 
fori  =  2:nl-l, 

M_tipl_diag(i,i)  =  mtl*ll'^2; 
end 

M_tipl_diag(l,l)  =  mtl*(ohl+ll)'^2; 

M_tipl_diag(nl,nl)  =  mtl*(ll+otl+ctl)'^2; 
for  i  =  2:nl-l, 

M_tip2_diag  1 1  (i,i)  =  mt2*l  1^2; 
end 

M_tip2_diagl  1(1,1)  =  mt2*(ohl+ll)'^2; 

M_tip2_diagl  l(nl,nl)  =  mt2*(ll+otl)^2; 
for  i  =  2:n2-l, 

M_tip2_diag22(i,i)  =  mt2*12^2; 
end 

M_tip2_diag22(l,l)  =  mt2*(oh2+12)^2; 

M_tip2_diag22(n2,n2)  =  mt2*(12+ot24et2)^2; 

M_tip2_diag  =  [M_tip2_diagll  zeros(nl,n2);zeros(n2,nl)  M_tip2_diag22]; 
M_hubs_off_diagll  =  zeros(nl,nl); 
for  i  =  2:nl-2, 
for  j  =  i+l;nl-l, 

M_hubs_off_diagl  l(i,j)  =  mb2*11^2; 
end 
end 

fori  =  2:nl-l, 

M_bubs_off_diagll(l,i)  =  mb2*(obl+ll)*ll; 

M_bubs_off_diagl  l(i,nl)  =  mb2*ll*(ll+otl); 
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end 

M_hubs_off_diagll(l,nl)  =  mh2*(ohl+ll)*(ll+otl); 

M_hubs_off_diag  11=  M_hubs_off_diag  1 1  +M_hubs_off_diag  11’; 
M_hubs_off_diagl2  =  zeros(nl,n2); 
for  i  =  2:nl-l, 

M_hubs_off_diagl2(i,l)  =  mh2*ll*ch2; 
end 

M_hubs_off_diag  12(1,1)  =  nih2*(ohl+ll)*ch2; 

M_hubs_off_diagl2(nl,l)  =  nih2*(ll+otl)*ch2; 

M_hubs_off_diag  =  [M_hubs_off_diagll  M_hubs_off_diagl2*cos(th2_nom); 

(M_hubs_off_diagl2’)*cos(th2_nom)  zeros(n2,n2)]; 
M_linkl_off_diagll  =  zeros(nl,nl); 
for  i  =  2:nl-l, 
forj  =  i+l;nl, 

M_linkl_off_diagll(i,j)  =  (l/2)*inll*11^2+(nl-j)*mll*11^2; 
end 
end 

fori  =  2;nl, 

M_linkl_off_diagll(l,i)  =  (l/2)*mll*(ohl+ll)*ll+(nl-i)*mll*(ohl+ll)*ll; 
end 

M_linkl_off_diagll  =  M_linkl_off_diagll+M_linkl_off_diagir; 
M_linkl_off_diag  =  [M_linkl_off_diagll  zeros(nl,n2);  zeros(n2,nl+n2)]; 
M_link2_off_diagll  =  zeros(nl,nl); 
fori  =  2:nl-2, 
for  j  =  i+l;nl-l, 

M_link2_off_diagl  l(i,j)  =  n2*ml2*11^2; 
end 
end 

fori  =  2:nl-l. 
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M_link2_off_diagll(l,i)  =  n2*ml2*(ohl+ll)*ll; 

M_link2_off_diagll(i,nl)  =  n2*ml2*ll*(ll+otl); 
end 

M_link2_off_diagll(l,nl)  =  n2*inl2*(ohl+ll)*(ll+otl); 

M_link2_off_diagl  1  =  M_link2_off_diagl  l+M_link2_off_diagir; 
for  i  =  2:nl-l, 
forj  =  2:n2, 

M_link2_off_diagl2(i,j)  =  (l/2)*ml2*ll*12+(n2-j)*ml2*ll*12; 
end 
end 

for  i  =  2:n2, 

M_link2_off_diagl2(l,i)  =  (l/2)*ml2*(ohl+ll)*12+(n2-i)*ml2*(ohl+ll)*12; 
end 

for  i  =  2:nl-l, 

M_link2_off_diag  1 2(i,  1 )  =  ml2*l  1  *(oh2+(12/2))+(n2- 1  )*ml2*l  1  *(oh2+12); 
end 

for  i  =  2:n2, 

MJink2_off_diagl2(nl,i)  =  (l/2)*nil2*(ll+otl)*12+(n2-i)*ml2*(ll+otl)*12; 
end 

M_link2_off_diagl2(l,l)  =  in]2*(ohl+ll)*(oh2+(12/2))+(n2- 

l)*ml2*(ohl+ll)*(oh2+12); 

MJink2_off_diagl2(nl,l)  =  nil2*(ll+otl)*(oh2+(12/2))+(n2- 

l)*nil2*(ll+otl)*(oh2+12); 

M_link2_off_diag22  =  zeros(n2,n2); 
fori  =  2:n2-l, 
for  j  =  i+l:n2, 

M_link2_off_diag22(i,j)  =  (l/2)*ml2*12^2+(n2-j)*ml2*12'^2; 
end 
end 

for  i  =  2:n2, 
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M_link2_off_diag22(  1  ,i)  =  ( l/2)*ml2*(oh2+12)*12+(n2-i)*ml2*(oh2+12)*12; 
end 

M_link2_off_diag22  =  M_link2_off_diag22+M_link2_off_diag22’; 
M_link2_off_diag  =  [M_link2_off_diagll  M_link2_off_diagl2*cos(th2_nom); 

(M_link2_off_diag  12’)  *cos(th2_nom)  M_lmk2_off_diag22] ; 
M_tipl_off_diagll  =  zeros(nl,nl); 
for  i  =  2;nl-2, 
for  j  =  i+l:nl-l, 

M_tipl_off_diagl  l(i,j)  =  mtl*11^2; 
end 
end 

for  i  =  2:nl-l, 

M_tipl_off_diagl  l(l,i)  =  mtl*(ohl+ll)*ll; 

M_tipl_off_diagll(i,nl)  =  mtl*ll*(ll+otl+ctl); 
end 

M_tipl_off_diagll(l,nl)  =  mtl*(ohl+ll)*(ll+otl+ctl); 

M_tipl_off_diagl  1  =  M_tipl_off_diagl  l+M_tipl_off_diagir; 
M_tipl_off_diag  =  [M_tipl_off_diagll  zeros(nl,n2);zeros(n2,nl+n2)]; 
M_tip2_off_diag  11=  zeros(n  1  ,n  1 ) ; 
for  i  =  2:nl-2, 
for  j  =  i+l:nl-l, 

M_tip2_off_diagl  l(i,j)  =  mt2*11^2; 
end 
end 

for  i  =  2:nl-l, 

M_tip2_off_diagll(l,i)  =  mt2*(ohl+ll)*ll; 

M_tip2_off_diagll(i,nl)  =  mt2*ll*(ll+otl); 
end 

M_tip2_off_diagll(l,nl)  =  mt2*(ohl+ll)*(ll+otl); 


M_tip2_off_diagl  1  =  M_tip2_off_diagl  l+M_tip2_off_diagir; 
M_tip2_off_diagl2  =  zeros(nl,n2); 
fori  =  2:nl-l, 
forj  =  2:n2-l, 

M_tip2_off_diagl2(i,j)  =  mt2*ll*12; 
end 

M_tip2_off_diagl2(i,l)  =  mt2*ll*(oh2+12); 

M_tip2_off_diagl2(i,n2)  =  mt2*ll*(12+ot2+ct2); 
end 

fori  =  2:n2-l, 

M_tip2_off_diagl2(l,i)  =  mt2*(ohl+ll)*12; 

M_tip2_off_diagl2(nl,i)  =  mt2*(ll+otl)*12; 
end 

M_tip2_off_diagl2(l,l)  =  nit2*(ohl+ll)*(oh2+12); 
M_tip2_off_diagl2(l,n2)  =  mt2*(ohl+ll)*(12+ot2+ct2); 
M_tip2_off_diagl2(nl,l)  =  mt2*(ll+otl)*(oh2+12); 
M_tip2_off_diagl2(nl,n2)  =  mt2*(ll+otl)*(12+ot2+ct2); 

M_tip2_off_diag22  =  zeros(n2,n2); 
for  i  =  2:n2-2, 
for  j  =  i+l:n2-l, 

M_tip2_off_diag22(i,j)  =  mt2*12^2; 
end 
end 

fori  =  2:n2-l, 

M_tip2_off_diag22(l,i)  =  mt2*(oh2+12)*12; 

M_tip2_off_diag22(i,n2)  =  mt2*12*(12+ot2+ct2); 
end 

M_tip2_off_diag22(l,n2)  =  mt2*(oh2+12)*(12+ot2+ct2); 

M_tip2_off_diag22  =  M_tip2_off_diag22+M_tip2_off_diag22’; 
M_tip2_off_diag  =  [M_tip2_off_diagll  (M_tip2_off_diagl2)*cos(th2_nom) 
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(M_tip2_off_diagl2’)*cos(th2_nom)  M_tip2_off_diag22]; 

M 

M_inertias+M_hubs_diag+M_linkl_diag+M_link2_diag+M_tipl_diag+M_tip2_di 

ag+M_hubs_off_diag+M_linkl_off_diag+M_link2_off_diag+M_tipl_off_diag+M 

_tip2_off_diag; 

%  state  transformation  of  mass  matrix  u  to  x 
for  i  =  l:(nl+n2), 
for  j  =  l:(nl+n2), 

Mbar(i,j)  =  sum(M(i,j:(nl+n2))); 
end 
end 

%  stiffness  matrix 
fori=  l:(nl-l), 

Kl(i,i+l)  =  -kl; 

Kl(i+l,i+l)  =  kl; 
end 

fori=  l:(n2-l), 

K2(i,i+1)  =  -k2; 

K2(i+l,i+l)  =  k2; 
end 

K=  [K1  zeros(nl,n2);zeros(n2,nl)  K2]; 

%  damping  matrix 
for  i  =  l;(nl-l), 

Dl(i,i+1)  =  -bl; 

Dl(i+l,i+l)  =  bl; 
end 

for  i  =  l:(n2-l), 

D2(i,i+1)  =  -b2; 

D2(i+l,i+l)  =  b2; 
end 
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D  =  [D1  zeros(nl,n2);zeros(n2,nl)  D2]; 

%  input  torque  matrix 
T  =  zeros(nl+n2,2); 

T(l,l)  =  l; 

T(nl,2)  =  -1; 

T(nl+l,2)=l; 

%  state  space  realization 
%  Mbar{d/dt(x)}  =  A{x}  +  T{u} 

%  {y}  =  C{x}  +  D{u} 

Mbar  =  [Mbar  zeros(nl+n2); 

zeros(n  1  +n2)  eye(n  1  +n2)] ; 

A=[-PD-1*K; 

eye(nl+n2)  zeros(nl+n2)]; 

T  =  [T;zeros(nl+n2,2)]; 

Abar  =  Mbar\A; 

Bbar  =  Mbar\T; 

C  =  [eye(2*(nl+n2)); 
zeros(2,2*(nl+n2))]; 

%  Stanford  model  had  error  in  the  term  relating  to  the  1st  element  of  each  link 
C(2*(n  1  +n2)+ 1  ,n  1  +n2+ 1 )  =  -(oh  1+1 1 )  *sin(th  l_nom)-(n  1  - 1  )*1 1  *sin(th  l_nom)- 
ot  1  *  sin(th  1  _nom)-(oh2+12)  *sin(th  1  _nom+th2_nom)-(n2- 
l)*12*sin(thl  _nom+th2_nom)-ot2  *  sin(th  1  _nom+th2_nom) ; 

cc  =  -(nl-l)*ll*sin(thl_nom)-otl*sin(thl_nom)- 

(oh2+12)*sin(thl_nom+th2_nom)-(n2-l)*12*sin(thl_nom+th2_nom)- 
ot2  *  sin(th  1  _nom+th2_nom) ; 
for  i  =  l:(nl-l), 

C(2*(nl+n2)+l,nl+n2+i+l)  =  cc-(l-i)*ll*sin(thl_nom); 
end 

C(2*(nl+n2)+l,nl+n2+nl+l)  =  -(oh2+12)*sin(thl_nom+th2_nom)-(n2- 
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l)*12*sin(thl_nom+th2_nom)-ot2*sin(thl_nom+th2_nom); 

cc  =  -(n2-l)*12*sin(thl_nom+th2_nom)-ot2*sin(thl_nom+th2_nom); 
for  i  =  l:(n2-l), 

C(2*(nl+n2)+l,nl+n2+i+nl+l)  =  cc-(l-i)*12*sin(thl_nom+th2_nom); 
end 

C(2*(nl+n2)+2,nl+n2+l)  =  (ohl+ll)*cos(thl_nom)+(nl- 

1  )*1 1  *cos(th  l_nom)+ot  1  *cos(th  l_nom)+(oh2+12)*cos(th  l_nom+th2_nom)+(n2- 
1  )*12*cos(th  1  _nom+th2_nom)+ot2  *cos(th  1  _nom+th2_nom) ; 

cc  =  (nl- 

1  )*1 1  *cos(th  l_nom)+ot  1  *cos(th  l_nom)+(oh2+12)  *cos(th  l_noni+th2_nom)+(n2- 
l)*12*cos(thl_nom+th2_nom)+ot2*cos(thl_nom+th2_nom); 
for  i  =  l:(nl-l), 

C(2*(nl+n2)+2,nl+n2+i+l)  =  cc+(l-i)*ll*cos(thl_nom); 
end 

C(2*(nl+n2)+2,nl+n2+nl+l)  =  (oh2+12)*cos(thl_noni+th2_nom)+(n2- 

l)*12*cos(thl_nom+th2_nom)+ot2*cos(thl_nom+th2_nom); 

cc  =  (n2-l)*12*cos(thl_nom+th2_nom)+ot2*cos(thl_nom+th2_nom); 
for  i  =  l:(n2-l), 

C(2*(nl+n2)+2,nl+n2+i+nl+l)  =  cc+(l-i)*12*cos(thl_nom+th2_nom); 
end 

D  =  zeros(2*(nl+n2)+2,2); 

%  end 

E.2  SANDY  Design  m.files 

The  following  two  m.files  are  used  in  the  SANDY  controller  design  process. 

E.2.1  sandyarm.m 

%  sandyarm.m 

%  load  plant  model,  and  form  sandy  synthesis  model 

% 

%  Full  order,  20  DOF 
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%  load  40ordarmev 
%  Reduced  order,  20  DOF  Evers 
%  load  ISordarmev 
%  reduced  order,  20  DOF  fast 
%  load  ISordarinhs 
%  Reducer  order,  20  DOF 
%  Written  by  Dave  Bossert 
%  Last  Revsion:  7  Oct  94 
load  SOordarm 

%  start  blocks  for  the  synthesis  model 

%  Block  1  -  Plant  (much  slower  than  actuators,  therefore  no  actuator  dynamics 
included) 
al=Abar; 
bl=Bbar; 
cl=C; 
dl=D; 

%  Block  2  -  Thetal  command  input 

n2=l; 

d2=l; 

%  Block  3  -  Theta2  command  input 

n3=l; 

d3=l; 

%  Block  4  -  Thetal  error  output 

n4=l; 

d4=l; 

%  Block  5  -  Theta2  error  output 

n5=l; 

d5=l; 

%  Build  the  synthesis  model 
nblocks=5; 
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blkbuild; 

%  inputs  -  delta_tl,  delta_t2,  tl_cmd,  t2_cmd 
inputs=[l,2,3,4]; 

%  40  DOF 

%  outputs  -  theta  1  dot,  theta2dot,  error_thetal,  error_theta2 
%  outputs=[l,21,85,86]; 

%Q=[5,41,-83; 

%  6,61,-84]; 

%  20  DOF  with  thetadots 
outputs=[  1 , 1 1 ,45,46] ; 

Q=[5,-21,43; 

6,-31,44]; 

%  20  DOF  without  thetadots 
%  outputs=[45,46]; 

%  Q=[5,2 1,-43; 

%  6,31,-44]; 

[Asynth,Bsynth,Csynth,Dsynth]=connect(a,b,c,d,Q,inputs,outputs); 
save  sandysynth  Asynth  Bsynth  Csynth  Dsynth 

E.2.2  sanamrin.m 

%sanarniin.m 

%  loads  SANDY  synthesis  model  and  generates  a  SANDY  data  file  for  use 
%  with  the  SANDY  design  software, 
format  short  e; 

%Load  synthesis  models 
load  sandysynth 
%  F  State  Matrices 
Fl=Asynth; 

F2=F1; 

%  G  Control  Input  Matrices 
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Gl=Bsynth(:,l:2); 

G2=G1; 

%  Gamma  Disturbance  Input  Matrices 
Gam  1  =B  synth( :  ,3 :4) ; 

Gam2=Gaml; 

%  Wspec  Disturbance  Inputs 
Wspecl=[0  0  1  1  1  0;00000  1]; 

Wspec2=[0  0  0  0  0  1;0  0  1  1  1  0]; 

%  He  Criterion  State  Matrix  -[Thetal_dot,  Theta2_dot, 
Thetal_emd,Theta2_cmd] 

Hcl=Csynth; 

Hc2=Hcl; 

%  Dcu  Criterion  Control  Input  Matrix 
Dcul=zeros(4,2);  %for  theta  dots 
%  Dcul=zeros(2,2); 

Deu2=Dcul; 

%  Dew  Criterion  Disturbance  Input  Matrix 
Dcwl=[0,0;0,0;l,0;0,l];  %for  theta  dots 
%Dcwl=[l,0;0,l]; 

Dcw2=Dcwl; 

%  Hs  Sensor  State  Matrix  -ys=[thetal_dot,  theta2_dot,  error_theta_l, 
error_theta_2] 

%Hsl=Csynth; 

%  Hs  Sensor  State  Matrix  -ys=[  error_theta_l,  error_theta_2] 
Hsl=Csynth(3:4,:); 

Hs2=Hsl; 

%  Dsu  Sensor  Control  Input  Matrix 
%Dsul=zeros(4,2);  %for  theta  dots 
Dsul=zeros(2,2); 

Dsu2=Dsul; 
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%  Dsw  Sensor  Disturbance  Input  Matrix 
%Dswl=[0,0;0,0;l,0;0,l];  %for  theta  dots 
Dswl=[l,0;0,l]; 

Dsw2=Dswl; 

%  Weighting  Matrices 
Ql=diag([0, 0,1,0]);%  for  theta  dots 
Q2=diag([0,0,0,l]); 

%  Weighting  Matrices 
%Ql=diag([l,0]); 

%Q2=diag([0,l]); 

Rl=diag([0,0]); 

R2=diag([0,0]); 

%Set  up  the  problem  for  SANDY 

sandygl 

Nfcmax=10; 

Tf=[25;25]; 

Tfctor=[2;2]; 

Npm=2; 

Wp=[l;l]; 

F=[F1;F2]; 

G=[G1;G2]; 

%Disturbance  inputs  w(t)=[Thetal_cmd,Theta2_cmd] 

Gam=  [Gam  1  ;Gam2] ; 

%Measurement  vector  ys=[thetal_dot,  theta2_dot,  error_theta_l,  error_theta_2] 
Hs=[Hsl;Hs2]; 

Dsu=  [Dsu  1  ;Dsu2] ; 

Dsw= [Dsw  1  ;Dsw2] ; 

%Criterion  vector  yc=[thetal_dot,  theta2_dot,  error_theta_l,  error_theta_2] 
%Criterion  vector  yc=[error_theta_l,  error_theta_2]  with  no  theta  dots 
Hc=[Hcl;Hc2]; 


Dcu=[Dcu  1  ;Dcu2] ; 

Dcw=  [Dew  1  ;Dc  w2] ; 

Wspec=  [Wspec  1  ;Wspec2] ; 

Alpha=[0;0]; 

Sigmamax=[-.02;  1  ] ; 

Zetainin=[0;0]; 

%Weighting  matrices  Q  and  R 
Q=[Q1;Q2]; 

R=[R1;R2]; 

nDircu=[0;0]; 

Dircu=[]; 

nDircy=[0;0]; 

Dircy=[]; 

%Initial  guess  (arbitrary)  of  the  LQG  controller  (with  theta  dots) 
A=[0,0;0,0]; 

B=[1,0;0,1]; 

C=[ 

-3.881  17.57; 

-2.235  10 

]; 

D=[ 

6.614-10.79 
1.052  2.647 

]; 

%Initial  guess  (arbitrary)  of  the  LQG  controller  (no  theta  dots) 
%A=[0,0;0,0]; 

%B=[1,0;0,1]; 

%C=[1.698,  2.348;  -0.1700,  -.235]; 

%D=[]; 

%  Design  Parameters 


nAid=0; 

Aid=[]; 

nBid=0; 

Bid=[]; 

nCid=4; 

Cid=[ 

1,1,1,-100,100 

1,2,1,-100,100 

2,1,1,-100,100 

2,2,1,-100,100]; 

%nDid=0; 

%Did=[]; 

%  with  theta  dot 
%nDid=8; 

%Did=[l, 1,1,-100,100; 

%  1,2,1,-100,100; 

%  1,3,1,-100,100; 

%  1,4,1,-100,100; 

%  2,1,1,-100,100; 

%  2,2,1,-100,100 

%  2,3,1,-100,100; 

%  2,4,1,-100,100]; 

%  without  theta  dot  in  sensors 
nDid=4; 

Did=[l, 1,1,-100,100; 

1,2,1,-100,100; 

2,1,1,-100,100; 

2,2,1,-100,100]; 

Nclin=0; 

nLincoef=[]; 
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Lincoef=[]; 

Linbnds=[]; 

Nudnlc=0; 
sandy  sv(  ‘  arminput’ ) 

E.3  General  Design 

This  section  presents  several  design  m.files  used  for  this  dissertation.  First,  Iq- 
garmdes.m  presents  LQG-class  controller  design  and  evaluation.  Also,  the  bw.m  file 
shows  the  bandwidth  generation  for  the  SANDY  and  PID  controllers. 

E.3.1  Iqgarmdes.m 

%  Iqgarmdes.m 
%  TLA  LQG  Design 
%  Written  by  Dave  Bossert 
%  Last  LFpdate  -  10  Oct  94 
%%%  Load  Synthesis  Model  %%% 
load  Iqgsynth 

%%%%%  LQR  Design  for  Gopt  %%%%% 

%%%  Use  only  theta_l  and  theta_2  inputs  %%% 

Blq  =  Bsynth(:,l:2); 

Dlq  =  Dsynth(:,l:2); 

alphal  =  1;  %%%  Theta_l  Tx  Zero  %%% 
alpha2  =  1000;  %%%  Theta_2  Tx  Zero  %%% 

%  Theta_l  Criterion 

Cl  =  Csynth(l,:);%  +  alphal*Csynth(3,:); 

D1  =  Dlq(l,:);%  +  alphal  *Dlq(3,:); 

%  Theta_2  Criterion 

C2  =  Csynth(2,:);%  +  alpha2*Csynth(4,:); 

D2  =  Dlq(2,:);%  +  alpha2*Dlq(4,:); 

%  Theta_l  Error 
C3  =  Csynth(5,:); 
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D3  =  Dlq(5,:); 

%  Theta_2  Error 
C4  =  Csynth(6,:); 

D4  =  Dlq(6,:); 

%  Criterion  Matrices  % 

Cyc=[Cl;C2;  C3;C4]; 

Dyc=[Dl;D2;  D3;D4]; 

%  Define  Weighting  Criterion  for  Q 
Q1  =  .001;%  penalty  on  Theta_l 
Q2  =  .002;  %  penalty  on  Theta_2 

Q3  =  50;  %  penalty  on  Theta_l  Error 

Q4  =  70;  %  penalty  on  Theta_2  Error 
Q  =  [Q1  0OO;0Q200;0OQ3  0;000Q4]; 

%  Control  Weighting  (R),  del_tl,  del_t2 
R1  =  .001;  %  del_tl  penalty 
R2  =  .001;  %  del_t2  penalty 
R  =  [R1  0;  0  R2]; 

%  Determine  Full-State  Feedback  Gain  (G) 

G  =  lqrcross(Asynth,Blq,Cyc,Dyc,Q,R); 

%  Partition  G  Matrix 
Ge  =  G(  1:2,1: 18);  %  Estimator  Gains 
Gi  =  G(  1:2, 19:20);  %  Integral  Gains 
%%%%%  LQG  Estimator  Design  %%%%% 

%%%  Define  Estimator  State  Space  %%% 

Ae  =  Asynth(l:18,l:18); 

Be  =  Bsynth(l:18,l:2); 

Gammae  =  Be; 

Ce  =  [Csynth(3,l:18);Csynth(4,l:18)]; 

%%%  Process  Noise  Matrix  %%% 

W1  =  20;  %0.01;  %20;  %%%  del_tl  Noise  (LTR) 


%%% 
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W2  =  20;  %0.01;  %20;  %%%  del_t2  Noise  (LTR) 
Wo  =  diag([Wl,W2]); 

%%%  Sensor  Noise  Matrix  %%% 

VI  =  1;  %%%  theta_l  noise  %%% 

V2  =  1;  %%%  theta_2  noise  %%% 

Vo  =  diag([Vl,V2]); 

Gff=[0  0;0  0]; 

%%%  Optimal  Gain,  K  %%% 

K  =  lqe(Ae,Gammae,Ce,Wo,Vo);%  Iqgarmdes.m 
%%%  Combined  LQG  Closed  Loop  System  %%% 
Acont  =  [Ae-K*Ce-Be*Ge,  -Be*Gi;zeros(2,20)]; 
H=[10;0  1]; 

I=eye(2); 

Bcont=[K,Be*Gff;-H,I]; 

Ccont  =  -G; 

Dcont  =  zeros(2,4); 

%%%  Evaluate  Estimator  &  Regulator  Poles  %%% 

damp(Ae-K*Ce); 

damp(Asynth  -  Blq*G); 

%%%  Evaluate  Command/Control  Bandwidths  %%% 
%  Input  3  -  th_lcmd.  Input  4  -  th_2cmd 
%%%  th_l/th_l_cmd  %%% 

Cx  =  [Csynth(3,:),  zeros(l,18);Csynth(4,:),zeros(l,18)]; 
Bx  =  [Bsynth(l:18,:);zeros(18,4);zeros(2,2),-eye(2)]; 
Dx  =  [Dsynth(3,;);Dsynth(4,:)]; 
pbode(  Acl,Bx,Cx,Dx,  1 ,3,.0 1 , 1 00); 

%  Controller  Reduction 
Aest=  [  Ae-K*Ce-Be  *Ge] ; 

Best=[-Be*Gi,K,Be*Gffl; 

Cest=-Ge; 


%%% 
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Dest=[-Gi,zeros(2,2),Gff]; 

%[ab,bb,cb,hksvl,T]=balinr(Acont,Bcont,Ccont,Dcont,3,12); 

%hksvl 

[  Ab,Bb,Cb,Db]=canon(  Aest,B  est,Cest,Dest,  ’  modal  ’ ) ; 
%[Ab,Bb,Cb,hsv,T]=balreal(Aest,Best,Cest); 

%hsv; 

elim=[l  2  3  4  5  6]; 

[Ared,Bred,Cred,Dred]=modred(Ab,Bb,Cb,Db,elim); 
n=  1 8-length(elim) ; 

Acont2=  [Ared,Bred(: ,  1  ;2);zeros(2,n+2)] ; 
Bcont2=[Bred(:,3:9);-H,I]; 

Ccont2= [Cred,Dred(: ,  1:2)]; 

Dcont2=  [Dred( :  ,3 : 9)] ; 

%%%  Block  Build  Reduced  LQG  Closed  Loop  System  %%% 
%%%  Block  1:  Synthesis  Model  (Plant)  %%% 
al  =  Ac; 
bl  =Bc; 
cl  =  Cc; 
dl  =Dc; 

%%%  Block  2:  LQR  Optimal  Gain  %%% 
a2  =  Acont2; 
b2  =  Bcont2; 
c2  =  Ccont2; 
d2  =  Dcont2; 

nblocks  =  2; 
blkbuild; 
iu  =  [3  4]; 

iy  =  [1  23  6  11  12  13]; 
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Q  =  [1,16,0,0 
2,17,0,0 
3,0,0,0 
4,0,0,0 
5,0,0,0 
6,0,0,0 
7,13,0,0 
8, 8,0,0 
9, 9,0,0 
10,10,0,0 
11,11,0,0 
12,0,0,0 
13,0,0,0]; 

[acl,bcl,Cl,Dl]  =  connect(a,b,c,d,Q,iu,iy); 

E.3.2  bw.m 

%  bw.m 

%  Calculates  Command  BW  for  a  plant  and  cascade  controller  (SANDY  or  PID) 

%  Written  by  Dave  Bossert,  Last  Update  - 10  Oct  94 

load  18ordarmbs 

a2=Abar; 

b2=Bbar; 

c2=[C(21,:);C(31,:)]; 

d2=[D(21,:);D(31,:)]; 

%SANDY 
%  al5good_c 
%  al=A; 

%  bl=B; 

%  cl=C; 

%  dl=D; 


%  PID,  1,.5, 2  (extra  pole  at  -10) 
al=[- 10,0,0,0;  1 ,0,0,0;0,0,- 10,0;0,0, 1 ,0]; 
bl=[l,0;0,0;0,l;0,0]; 
cl=[-199.5,5,0,0;0,0,-199.5,5]; 
dl=[21,0;0,21]; 

[aol,bol,col,(iol]=series(al  ,b  1  ,c  1  ,dl  ,a2,b2,c2,d2); 

acl=(aol-bol*col); 

pbode(acl,bol,col,dol,  1, 1 , 1 , 1000) 


VITA 


David  Edward  Bossert 

Birthdate:  October  11, 1962 

Parents:  Edward  F.  and  Patricia  A.  Bossert 

Advanced  Degrees: 

Bachelor  of  Science,  Electrical  Engineering, 
University  of  Missouri-Columbia,  1984 

Master  of  Science,  Systems  Management, 
University  of  Southern  California,  1987 

Master  of  Science,  Electrical  Engineering, 

Air  Force  Institute  of  Technology,  1989 

Doctor  of  Philosophy,  Aeronautics  and  Astronautics 
University  of  Washington,  1996 


